/*
 * Dalek.cpp
 *
 *  Created on: Feb 29, 2012
 *      Author: baneling
 */

#include "Dalek.h"
#include <ctime>
#define randomize srand((unsigned)time(NULL))
#define random(x) rand() % x
using namespace PlayerCc;

Dalek::Dalek(string name,string address, uint32_t positionProxyIndex,uint32_t sonarProxyIndex,uint32_t bloblfinderProxyIndex, uint32_t simulationProxyIndex)
{

	this->svolteCount=0;
	this->dalekName=name;
	if(this->dalekName!="dalekCan")
	{
		otherstates_t dataCan;
		dataCan.name="dalekCan";
		dataCan.state=FORMATIONGO;
		this->dalekNames.push_back(dataCan);
	}
	if(this->dalekName!="dalekSec")
	{
		otherstates_t dataSec;
		dataSec.name="dalekSec";
		dataSec.state=FORMATIONGO;
		this->dalekNames.push_back(dataSec);
	}
	if(this->dalekName!="dalekThay")
	{
		otherstates_t dataThay;
		dataThay.name="dalekThay";
		dataThay.state=FORMATIONGO;
		this->dalekNames.push_back(dataThay);
	}
	if(this->dalekName!="dalekJast")
	{
		otherstates_t dataJast;
		dataJast.name="dalekJast";
		dataJast.state=FORMATIONGO;
		this->dalekNames.push_back(dataJast);
	}

	this->itemList.name= "who" ;

	size_t pos=address.find_first_of(":");
	if(pos!=string::npos){
		string port=address.substr(pos+1);
		uint32_t portnumber=atoi(port.c_str());
		address=address.substr(0,pos);
		this->robotClient=new PlayerClient(address,portnumber);
	}else{
		this->robotClient=new PlayerClient(address);
	}

	this->p2dProxy=new Position2dProxy(this->robotClient, positionProxyIndex);
	this->sonarProxy=new SonarProxy(this->robotClient,sonarProxyIndex);
	this->blobProxy=new BlobfinderProxy(this->robotClient,bloblfinderProxyIndex);
	this->simProxy= new SimulationProxy(this->robotClient,simulationProxyIndex);
	this->oldFormationYaw=0;
	this->RefreshItemList();
	this->drDistance=1.75;
	this->waitinformAnsCounter=0;
	this->reachlocationCounter=0;
	this->aroundCount=0;
	this->circumnavigateCount=0;
	this->allontanamentoCount=0;
	this->avoiding=false;
	this->imNavigator=false;
	this->bestsofar=100;
	this->initObstaclesPosition(this->obstaclesPosition);
	packet_t packet1;
	packet1.isRead=true;
	packet_t packet2;
	packet2.isRead=true;
	packet_t packet3;
	packet3.isRead=true;
	packet_t packet4;
	packet4.isRead=true;
	packet_t packet5;
	packet5.isRead=true;
	packet_t packet6;
	packet6.isRead=true;
	this->packetRead.push_back(packet1);
	this->packetRead.push_back(packet2);
	this->packetRead.push_back(packet3);
	this->packetStateRead.push_back(packet4);
	this->packetStateRead.push_back(packet5);
	this->packetStateRead.push_back(packet6);
	this->counterYaw=0;
	this->wantedYaw=0;
	this->pushDr=false;
	this->nearDoc=false;
	this->iCanPush=false;
	this->formationPlace.x=-1000;

	this->formationOrientationFlag=noaction;
}

Dalek::~Dalek()
{
	delete this->robotClient;
	delete this->sonarProxy;
	delete this->blobProxy;
	delete this->p2dProxy;
	delete this->simProxy;
	this->robotClient=NULL;
	this->p2dProxy=NULL;
	this->sonarProxy=NULL;
	this->blobProxy=NULL;
	this->p2dProxy=NULL;
	this->simProxy=NULL;
	this->dalekNames.empty();
}

void Dalek::writeOnPipe(enum message_t mess,string receiver,double x, double y)//go to Pipe::writeOnPipe
{
	for(list<dalekPipe_t>::iterator it = pipeWrite.begin(); it != pipeWrite.end(); it++)
	{
		if(((*it).dalekReader==receiver)||(receiver==SEND_TO_ALL))
		{
			//cout<<this->dalekName<<" sending message: "<<Pipe::enumToStr(mess)<<" to: "<<receiver<<endl;
			Pipe::writeOnPipe((*it).pipeId,this->dalekName,receiver,mess,x,y);
		}
	}
}

bool Dalek::readOnPipe() //go to Pipe::readOnPipe
{

	bool isMessageIn=false;
	list <packet_t>::iterator rec=this->packetRead.begin();
	msg_t mess;
	for(list<int>::iterator it = pipeRead.begin(); it != pipeRead.end(); it++)
	{
		mess=Pipe::readOnPipe(*it);
		if(mess.m_t!=none)
		{
			if((mess.receiver==this->dalekName) || (mess.receiver==SEND_TO_ALL))
			{
				//cout << this->dalekName << " msg: "<<Pipe::enumToStr(mess.m_t)<<" received - from: "<< mess.sender <<endl;
				(*rec).message=mess;
				(*rec).receiver_id=*it;
				(*rec).isRead=false;
				isMessageIn=true;
				rec++;
			}
		}
	}
	return isMessageIn;
}

void Dalek::writeOnStatePipe(enum message_t mess,string receiver,double x, double y)//go to Pipe::writeOnPipe
{
	for(list<int>::iterator it = pipeStateWrite.begin(); it != pipeStateWrite.end(); it++)
	{
		Pipe::writeOnPipe(*it,this->dalekName,receiver,mess,x,y);
	}
}

bool Dalek::readOnStatePipe() //go to Pipe::readOnPipe
{
	bool isMessageIn=false;
	list <packet_t>::iterator rec=this->packetStateRead.begin();
	msg_t mess;
	for(list<int>::iterator it = pipeStateRead.begin(); it != pipeStateRead.end(); it++)
	{
		mess=Pipe::readOnPipe(*it);
		if(mess.m_t!=none)
		{
			if((mess.receiver==this->dalekName) || (mess.receiver==SEND_TO_ALL))
			{
				//cout << this->dalekName << " state msg "<<mess.m_t<<" received" <<endl;
				(*rec).message=mess;
				(*rec).receiver_id=*it;
				(*rec).isRead=false;
				isMessageIn=true;
				rec++;
			}
		}
	}
	return isMessageIn;
}

void Dalek::setStatePipes(int pWrite,int pRead)
{
	this->pipeStateWrite.push_back(pWrite);
	this->pipeStateRead.push_back(pRead);
}

void Dalek::setPipes(int pWrite,int pRead,string pReader)
{
	dalekPipe_t write;
	dalekPipe_t read;
	write.pipeId=pWrite;
	write.dalekReader=pReader;
	write.dalekWriter=this->dalekName;
	this->pipeWrite.push_back(write);
	this->pipeRead.push_back(pRead);
}

void Dalek::run()
{
	int contatoreNavigatore=5;
	this->robotClient->Read();
	//TODO servono solo per un test, rimuovere:
	//int nextYaw;
	//bool goOnX;const int SIDEMARGIN=1;
	//fine
	bool obtsFlag=false;
	uint32_t targetcolor=ORANGE;
	double temperature=0.9;
	bool foundDrFlag=false, push=false;
	//double forwardSpeed =0, turnSpeed=0;
	this->p2dProxy->SetMotorEnable(1);
	this->p2dProxy->RequestGeom();
	this->sonarProxy->RequestGeom();
	int counter=0;
	int obstacleCount=0;
	double robotX, robotY, robotYaw;
	//int useless=0;
	double pldist;
	string dalekErrorState;
	//solo per test!
	/*if(this->dalekName=="dalekCan")
	{
		this->formationPlace.x=9;
		this->formationPlace.y=4.25;
		this->imNavigator=true;

	}
	if(this->dalekName=="dalekSec")
	{
		this->formationPlace.x=9;
		this->formationPlace.y=7.75;
	}
	if(this->dalekName=="dalekThay")
	{
		this->formationPlace.x=9;
		this->formationPlace.y=4.25;
	}
	if(this->dalekName=="dalekJast")
	{
		this->formationPlace.x=10.75;
		this->formationPlace.y=6;
	}
	this->oldFormationYaw=270;
	this->formationYaw=270;
	this->pushDr=false;
	this->state=FORMATIONGO;*/
	this->state=WANDERING;
	this->broadcastStateFunction(this->state);
	//this->drFree=true;
	//this->state=PaskTest;
	//this->checkObstacles();
	this->simProxy->GetPose2d((char*)"who", this->docCoord.x,this->docCoord.y,this->docCoord.yaw);
	this->simProxy->GetPose2d((char*)"tardis", this->cabCoord.x,this->cabCoord.y,this->cabCoord.yaw);
	//fine solo per test
	bool debug=false;
	while(true)
	{
		this->handleTarget(&foundDrFlag, &push);
		if(this->dalekName=="dalekCan")//timestamp
		{
			// current date/time based on current system
			time_t now = time(0);

			// convert now to string form
			char* dt = ctime(&now);

			cout << "The local date and time is: " << dt << endl;
		}
		counter++;
		if(counter==10)//alivemessage
		{
			counter=0;
			cout<<this->dalekName<< " alive and in state " << this->stateToString(this->state) <<" #"<<this->state<<" oldYaw: "<<this->oldFormationYaw<<" yaw: "<<this->formationYaw<<endl;
			this->broadcastStateFunction(this->state);
		}

		if(!debug)//lettura messaggi pipe
		{
			this->prevState=this->state;
			if(this->readOnStatePipe())
			{

				for(list <packet_t>::iterator rec=this->packetStateRead.begin(); rec!=this->packetStateRead.end();rec++)
				{
					if (!(*rec).isRead) {
						switch((*rec).message.m_t)
						{
						case askState:
							this->writeOnStatePipe(ansState,(*rec).message.sender,this->state,0);
							break;
						case ansState:
							updateStateFunction((*rec).message.sender, (*rec).message.x);
							break;
						default:
							//updateStateFunction((*rec).message.sender, (*rec).message.x);
							//todo togli questo update e verifica l'errore
							cout<<"ERROR: message "<<Pipe::enumToStr((*rec).message.m_t)<<" read on statePipe!"<<endl;
							break;
						}
						(*rec).isRead=true;
					}
				}
			}

			this->handleTarget(&foundDrFlag, &push);
			if(this->readOnPipe())//lettura pipe
			{
				for(list <packet_t>::iterator rec=this->packetRead.begin(); rec!=this->packetRead.end();rec++)
				{
					if (!(*rec).isRead) {
						switch((*rec).message.m_t)
						{
						case sendFormationYaw:
							if((*rec).message.x<500){
								if((this->state==FORMATIONGO)||(this->state==PaskTest))
								{
									this->oldFormationYaw=this->formationYaw;
									this->formationYaw=(*rec).message.x;
									cout << "----------------"<<endl;
									cout << this->dalekName << " RECEIVED FORMATION_YAW: "<< this->formationYaw<<endl;
									cout << "----------------"<<endl;
									this->formationOrientationFlag=correctposition;
									this->state=FORMATIONORIENTATION;
									this->broadcastStateFunction(this->state);
									this->pushDr=false;
								}
								else if((this->state==FORMATIONORIENTED)||(this->state==FORMATIONORIENTATION))//todo check
								{
									if(this->formationYaw!=(*rec).message.x)
									{
										this->oldFormationYaw=this->formationYaw;
										this->formationYaw=(*rec).message.x;
										cout << "----------------"<<endl;
										cout << this->dalekName << " RECEIVED FORMATION_YAW: "<< this->formationYaw<<" con stato: "<<this->state<<endl;
										cout << "----------------"<<endl;
										this->formationOrientationFlag=correctposition;
										this->state=FORMATIONORIENTATION;
										this->broadcastStateFunction(this->state);
										this->pushDr=false;
									}
									else
										cout<<this->dalekName<<" received his formation again "<<(*rec).message.x<<" from "<<(*rec).message.sender<<endl;
								}
								cout<<this->dalekName<<" discarded formation yaw STATO: "<<this->stateToString(this->state)<<endl;
							}
							else
							{
								cout<<this->dalekName<<" entra in pasktest!!!"<<endl;
								this->state=PaskTest;
								this->broadcastStateFunction(this->state);
							}
							break;
						case goToStop:
							this->state=STOP;
							this->broadcastStateFunction(this->state);
							break;
						case drFound:
							this->state=DOCFOUND;
							this->broadcastStateFunction(this->state);
							this->docCoord.x=(*rec).message.x;
							this->docCoord.y=(*rec).message.y;
							break;
						case cabFound:
							this->cabCoord.x=(*rec).message.x;
							this->cabCoord.y=(*rec).message.y;
							if ((this->state!=CHECKDRPOSITION)
									&&(this->state!=FORMATIONFARAWAY)
									&&(this->state!=WAITOTHERSAWAY))
							{
								this->state=CABFOUND;
								this->broadcastStateFunction(this->state);
							}
							break;
						case shift:
							this->simProxy->GetPose2d((char*)"who",this->docCoord.x,this->docCoord.y,this->docCoord.yaw);
							this->simProxy->GetPose2d((char*)this->dalekName.c_str(), this->myCoord.x,this->myCoord.y,this->myCoord.yaw);

							cout<<this->dalekName<<"shift request ricevuto: "<<(*rec).message.x<<","<<(*rec).message.y<<endl;
							cout<<this->dalekName<<"obiettivo: "<<this->formationPlace.x<<","<<this->formationPlace.y<<endl;
							if((fabs(this->formationPlace.x-(*rec).message.x)<0.5)&&(fabs(this->formationPlace.y-(*rec).message.y)<0.5))
							{
								this->state=FORMATION;
								this->broadcastStateFunction(this->state);
								this->writeOnPipe(okshift,(*rec).message.sender,this->formationPlace.x,this->formationPlace.y);
								this->getDalekDir();
								this->formationPlace.x=getFormationCoord(getNextDir(),1);
								this->formationPlace.y=getFormationCoord(getNextDir(),0);
								cout<< this->dalekName<<" nuovo obiettivo: "<<this->formationPlace.x<<","<<this->formationPlace.y<<endl;
							}
							else
							{
								writeOnPipe(noshift,(*rec).message.sender,0,0);
							}
							break;
						case okshift:
							if(this->state==WAITINFORMANSWER)
							{

								this->state=FORMATION;
								this->broadcastStateFunction(this->state);

								this->formationPlace.x=(*rec).message.x;
								this->formationPlace.y=(*rec).message.y;

								//cout<<this->dalekName << "received okack"<< " changed formationPlace to ("<<this->formationPlace.x<<","<<this->formationPlace.y<<") "<<std::endl;
								sleep(3);
							}
							break;
						case noshift:
							if(this->state==WAITINFORMANSWER)
							{
								//cout<<this->dalekName << "received prrrrrack"<<std::endl;
								this->state = FORMATION;
								sleep(5);
							}
							break;
						case awayFromDoc:
							if(this->state==WAITOTHERSAWAY)
							{
								this->state=CHECKDRPOSITION;
								this->broadcastStateFunction(this->state);
							}
							break;
						case obstacles:
							int x,y;
							//cout<< this->dalekName<< " ricevuto obstacles da "<< (*rec).message.sender<<endl;
							if((*rec).message.sender=="dalekCan")
							{
								this->obstaclesPosition.can=1;
								//cout<< this->dalekName<<" aggiornato .can a "<<this->obstaclesPosition.can<<endl;
							}
							if((*rec).message.sender=="dalekSec")
							{
								this->obstaclesPosition.sec=1;
								//cout<< this->dalekName<<" aggiornato .sec a "<<this->obstaclesPosition.sec<<endl;
							}
							if((*rec).message.sender=="dalekThay")
							{
								this->obstaclesPosition.thay=1;
								//cout<< this->dalekName<<" aggiornato .thay a "<<this->obstaclesPosition.thay<<endl;
							}
							if((*rec).message.sender=="dalekJast")
							{
								this->obstaclesPosition.jast=1;
								//cout<< this->dalekName<<" aggiornato .jast a "<<this->obstaclesPosition.jast;
							}

							y=(int)(*rec).message.x;
//							if(this->imNavigator)
//								cout<<this->dalekName<<" RICEVUTI OSTACOLI: "<<y<<" from: "<<(*rec).message.sender<<endl;
							x=y % 1000;
							y-=x;
							if(y!=0) this->obstaclesPosition._0=true;
							y=x;
							x=y % 100;
							y-=x;
							if(y!=0) this->obstaclesPosition._90=true;
							y=x;
							x=y % 10;
							y-=x;
							if(y!=0) this->obstaclesPosition._180=true;
							if(x!=0) this->obstaclesPosition._270=true;
//							if(this->imNavigator)
//							{
//								cout<<this->dalekName<<" IN RICEZIONE"<<endl<<"0: "<<this->obstaclesPosition._0<<" 90: "<<this->obstaclesPosition._90<<
//										" 180: "<<this->obstaclesPosition._180<<" 270: "<<this->obstaclesPosition._270 <<endl;
//							}
							break;
						case drIsFree:
							this->drFree=true;
							break;
						case goFormation:
							this->state=FORMATION;
							this->broadcastStateFunction(this->state);

							this->formationPlace.x=(*rec).message.x;
							this->formationPlace.y=(*rec).message.y;
							break;
						default:
							cout<<this->dalekName<<" problem on switchformationyaw in run"<<endl;
							break;
						}
					}

					(*rec).isRead=true;
				}
			}
			this->handleTarget(&foundDrFlag, &push);
			//selettore comportamenti
		}
		else
		{
			cout<<this->dalekName<< " alive and in state " << this->stateToString(this->state) <<" #"<<stateToString(this->state)<<endl;
		}
		switch(this->state)
		{
		case FORMATIONORIENTED:
			if(!this->checkEveryoneState(FORMATIONORIENTED,FORMATIONGO))
			{
				cout<<this->dalekName<<" aspettando godot FORMATIONORIENTED"<<endl;
				sleep(3);
				this->broadcastStateFunction(this->state);
			}
			else
			{
				this->svolteCount++;
				cout<<this->dalekName<<"<<<<<<<<<<<svoltecount:::: "<<this->svolteCount<<endl;
				this->oldFormationYaw=this->formationYaw;
				this->state=FORMATIONGO;
				this->broadcastStateFunction(FORMATIONGO);
			}
			break;
		case FORMATIONORIENTATION:
			this->forwardspeed=0;
			this->turnspeed=0;
			this->pushDr=false;
			this->p2dProxy->SetSpeed(this->forwardspeed,dtor(this->turnspeed));
			this->simProxy->GetPose2d((char*)"who", this->docCoord.x,this->docCoord.y,this->docCoord.yaw);
			this->simProxy->GetPose2d((char*)this->dalekName.c_str(), this->myCoord.x,this->myCoord.y,this->myCoord.yaw);

			//todo sostituire il while con uno switch di modalità
			//modalità 1: while !stopincorrectposition
			//while(!stopInCorrectPosition())
			if(this->formationOrientationFlag==correctposition)
			{
				//cout<<this->dalekName<<" stoppandosi"<<endl;
				if(!this->stopInCorrectPosition())
				{

					this->pushDr=false;
					this->p2dProxy->SetSpeed(this->forwardspeed,dtor(this->turnspeed));
					sleep(1);
					//this->simProxy->GetPose2d((char*)"who", this->docCoord.x,this->docCoord.y,this->docCoord.yaw);
					//this->simProxy->GetPose2d((char*)this->dalekName.c_str(), this->myCoord.x,this->myCoord.y,this->myCoord.yaw);
				}
				else
				{
					this->formationOrientationFlag=turnyaw;
					//cout<<this->dalekName<<"stop is correct, now turning to: "<<this->formationYaw<<endl;//TODO leva
				}
			}

			//cout<<this->dalekName<<"stop is correct, now turning to: "<<this->formationYaw<<endl;//TODO leva
			//modalità 2: while !turntoyawglobalsafe
			//while(!this->turnToYawGlobalSafe(this->formationYaw))
			if(this->formationOrientationFlag==turnyaw){
				if(!this->turnToYawGlobalSafe(this->formationYaw))
				{
					//this->simProxy->GetPose2d((char*)"who", this->docCoord.x,this->docCoord.y,this->docCoord.yaw);
					//this->simProxy->GetPose2d((char*)this->dalekName.c_str(), this->myCoord.x,this->myCoord.y,this->myCoord.yaw);

					this->p2dProxy->SetSpeed(this->forwardspeed,dtor(this->turnspeed));
					sleep(1);
				}
				else
				{
					this->formationOrientationFlag=formationoriented;
				}
			}
			if(this->formationOrientationFlag==formationoriented)
			{
				this->p2dProxy->SetSpeed(0,0);
				//cout<<this->dalekName<<" turned for good now oriented"<<endl; //todo leva
				this->state=FORMATIONORIENTED;
				this->broadcastStateFunction(this->state);
				this->formationOrientationFlag=noaction;
			}
			if(this->imNavigator)
			{
				dalekErrorState=this->checkEveryoneState(FORMATIONORIENTATION);
				if(dalekErrorState!="OK")
				{
					cout<<dalekErrorState<<" was not aware of FYAW: "<<this->formationYaw<<endl;
					//manda messaggio di cambio stato a dalekErrorState
					this->writeOnPipe(sendFormationYaw,dalekErrorState,this->formationYaw,-1);
				}
			}
			break;
		case DOCFOUND://someone else found the doctor
			cout << this->dalekName << " someone else has found the doctor"<<endl;
			while(!(this->nearDoc=this->goAwayFromDoctor()))
			{
				this->avoidObstacles(&this->forwardspeed,&this->turnspeed,*this->sonarProxy,false);
			}
			if(this->nearDoc)
			{
				targetcolor=PINK;
				this->state=WANDERING;
				this->broadcastStateFunction(this->state);
			}
			break;
		case SEENCABIN:
			this->checkCabin();
			break;
		case CABFOUND://someone else found the cabin
			cout << this->dalekName << " someone else has found the cabin"<<endl;
			if(this->prevState!=CHECKDRPOSITION)
			{
				this->state=FORMATIONFARAWAY;
				//andare in stato che tutti vadano in formazione
				//questo deve essere uno stato in cui si attende che il dr
				//sia stato circumnavigato e che sia libero
				//quindi uno stato di attesa!
				this->broadcastStateFunction(this->state);
			}
			else
			{
				this->state=this->prevState;
				this->broadcastStateFunction(this->state);
			}
			break;
		case IFOUNDDR:
			cout << this->dalekName << " has found the doctor"<< endl;
			this->writeOnPipe(drFound,SEND_TO_ALL,this->docCoord.x,this->docCoord.y);
			this->p2dProxy->SetSpeed(this->forwardspeed, dtor(this->turnspeed));
			//this->state=CHECKDRPOSITION;
			this->drFree=false;
			this->state=WAITOTHERSAWAY;
			this->broadcastStateFunction(this->state);
			break;
		case WAITOTHERSAWAY:
			if(this->noneIsAwain())
			{
				this->state=CHECKDRPOSITION;
				this->broadcastStateFunction(this->state);
			}
			break;
		case IFOUNDCAB:
			cout << this->dalekName << " found cabin"<< endl;
			this->writeOnPipe(cabFound,SEND_TO_ALL,cabCoord.x,cabCoord.y);

			sleep(5);

			if(this->drFree)
			{
				if(this->dalekName!="dalekCan")
				{
					this->writeOnPipe(goFormation,"dalekCan",this->getFormationCoord(top,true),this->getFormationCoord(top,false));
				}
				if(this->dalekName!="dalekSec")
				{
					this->writeOnPipe(goFormation,"dalekSec",this->getFormationCoord(bot,true),this->getFormationCoord(bot,false));
				}
				if(this->dalekName!="dalekJast")
				{
					this->writeOnPipe(goFormation,"dalekJast",this->getFormationCoord(sx,true),this->getFormationCoord(sx,false));
				}
				if(this->dalekName!="dalekThay")
				{
					this->writeOnPipe(goFormation,"dalekThay",this->getFormationCoord(dx,true),this->getFormationCoord(dx,false));
				}
				this->state=FORMATIONFARAWAY;
				this->broadcastStateFunction(this->state);
			}
			else
			{
				usleep(500000);
			}
			break;
		case SEENDOCTOR:
			this->checkDoctor();
			break;
		case CHECKDRPOSITION:
			//circumnavigare il dottore per controllare se c'e' spazio per la formazione
			if(this->circumnavigateDr())
				this->circumnavigateCount++;
			else
			{
				this->state=STOP;//andare in stato di attesa che tutti vadano in formazione!
				this->broadcastStateFunction(this->state);
			}
			break;
		case WANDERING:
			/* find the robot...
			 * now we find the centre of the search circle.
			 * this is distBotToCircle metres from the robot’s origin
			 * along its yaw
			 * horizontal offset from robot origin
			 */
			//cout << " wandering with color: "<<targetcolor<<endl;
			this->wanderingState(foundDrFlag,targetcolor,push,temperature);
			break;
		case FORMATION:
		case FORMATIONFARAWAY:
			this->simProxy->GetPose2d((char*)"who",this->docCoord.x,this->docCoord.y,this->docCoord.yaw);
			this->simProxy->GetPose2d((char*)this->dalekName.c_str(), this->myCoord.x,this->myCoord.y,this->myCoord.yaw);
				if(this->formationPlace.x>-999)
			{
				reachFormationPlace(*this->simProxy,this->forwardspeed,this->turnspeed);
			}
			else
			{
				usleep(500000);
			}
			break;
		case WAITINFORMATION:

			this->simProxy->GetPose2d((char*)this->dalekName.c_str(), robotX, robotY, robotYaw);
			pldist=sqrt(((this->formationPlace.x-robotX)*(this->formationPlace.x-robotX))+((this->formationPlace.y-robotY)*(this->formationPlace.y-robotY)));
			if(pldist>0.6)
			{
				this->state=FORMATION;
				this->broadcastStateFunction(this->state);
			}
			sleep(1);
			break;
		case WAITINFORMANSWER:
			this->waitinformAnsCounter+=1;
			if(this->waitinformAnsCounter>=5)
			{
				this->state=FORMATION;
				this->broadcastStateFunction(this->state);
				this->waitinformAnsCounter=0;
			}
			else
				sleep(3);
			break;
		case STOP:
			this->p2dProxy->SetSpeed(0, dtor(100));
			sleep(10);
			break;
		case SENSORTEST://todo delete this!
			if(this->dalekName=="dalekCan")
			{
				this->simProxy->GetPose2d((char*)this->dalekName.c_str(), this->myCoord.x,this->myCoord.y,this->myCoord.yaw);reachLocation(3,0,*this->simProxy,0,&this->forwardspeed, &this->turnspeed);
				cout<<"reach-> v: "<<this->forwardspeed<<" a: "<<this->turnspeed;
				temperature=this->turnspeed;
				avoidObstacles(&this->forwardspeed,&this->turnspeed,*this->sonarProxy,push);
				cout<<" # avoid-> v: "<<this->forwardspeed<<" a: "<<this->turnspeed<< endl;
				if(temperature!=this->turnspeed)
					cout<<"variazione #####0"<<endl;
				this->p2dProxy->SetSpeed(this->forwardspeed, dtor(this->turnspeed));
				this->turnspeed=0;
				this->robotClient->Read();
				for (int i=0;i<10;i++) //tenere coerente con dalek.inc, 0 e 1 sono i tooth e vanno esclusi
				{
					if(this->sonarProxy->GetScan(i)<2)
					{
						cout << "visto!!!" << this->sonarProxy->GetScan(i) <<"\n";
						cout<<endl;
					}
				}
				reachLocation(3,0,*this->simProxy,0,&this->forwardspeed, &this->turnspeed);
				cout<<"reach-> v: "<<this->forwardspeed<<" a: "<<this->turnspeed;
				temperature=this->turnspeed;
				avoidObstacles(&this->forwardspeed,&this->turnspeed,*this->sonarProxy,push);
				cout<<" # avoid-> v: "<<this->forwardspeed<<" a: "<<this->turnspeed<< endl;
				if(temperature!=this->turnspeed)
					cout<<"variazione #####0"<<endl;
				this->p2dProxy->SetSpeed(this->forwardspeed, dtor(this->turnspeed));
				this->turnspeed=0;
			}
			sleep(1);
			break;
		case FORMATIONGO:
			this->simProxy->GetPose2d((char*)"who",this->docCoord.x,this->docCoord.y,this->docCoord.yaw);
			this->simProxy->GetPose2d((char*)"tardis", this->cabCoord.x,this->cabCoord.y,this->cabCoord.yaw);
			this->simProxy->GetPose2d((char*)this->dalekName.c_str(), this->myCoord.x,this->myCoord.y,this->myCoord.yaw);

			if(this->svolteCount>=SOGLIASVOLTE)
			{

				this->checkRole();
				if(this->svolteCount==SOGLIASVOLTE)
				{
					//gira tutto a dex
					if (this->imNavigator)
					{
						cout<<this->dalekName<<" >>>>>>>> aggiusting 1: "<<0<<endl;
						this->writeOnPipe(sendFormationYaw,SEND_TO_ALL,0,-1);
						this->formationOrientationFlag=correctposition;
						this->oldFormationYaw=this->formationYaw;
						this->formationYaw=0;
						this->state=FORMATIONORIENTATION;
						this->broadcastStateFunction(this->state);
					}
					this->forwardspeed=0;
					this->turnspeed=0;
				}
				else
				{
					//gira a top e resetta
					if (this->imNavigator)
					{
						cout<<this->dalekName<<" >>>>>>>>  aggiusting 2: "<<90<<endl;
						this->writeOnPipe(sendFormationYaw,SEND_TO_ALL,90,-1);
						this->formationOrientationFlag=correctposition;
						this->oldFormationYaw=this->formationYaw;
						this->formationYaw=90;
						this->state=FORMATIONORIENTATION;
						this->broadcastStateFunction(this->state);
					}
					this->forwardspeed=0;
					this->turnspeed=0;
					this->svolteCount=0;
				}
			}
			else
			{
				this->handleTarget(&foundDrFlag, &push);
				this->checkRole();
				//this->checkObstacles();
				//			if(this->imNavigator)
				//				cout<<"SOMMA FLAG OSTACOLI: "<<this->obstaclesPosition.can+this->obstaclesPosition.sec+this->obstaclesPosition.jast+this->obstaclesPosition.thay<<endl;
				if(this->obstaclesPosition.can+this->obstaclesPosition.sec+this->obstaclesPosition.jast+this->obstaclesPosition.thay>=3)
				{
					if (this->imNavigator)
					{
						cout<<"==> NAVIGATOR <== >>"<<contatoreNavigatore<<endl;
						if(contatoreNavigatore>=20)
						{
							contatoreNavigatore=0;
							cout<<"NAVIGATING!"<<endl;
							this->navigate();
						}
						else
						{
							//contatoreNavigatore++;
							cout<<"EASY NAVIGATE"<<endl;
							contatoreNavigatore=this->easyNavigate(contatoreNavigatore);
						}
					}

					//resetting obstaclesPosition
				}
				else
				{
					if(this->imNavigator)
						cout<< "NAVIGAZIONE impossibru"<<endl<<this->obstaclesPosition.can+this->obstaclesPosition.sec+this->obstaclesPosition.jast+this->obstaclesPosition.thay<<endl;
				}
				if(this->state==FORMATIONGO)
				{
					switch(this->formationKeeper())
					{
					case 1://in avanti
						cout<<this->dalekName<<" Formation keeper: AVANTI!"<<endl;
						this->forwardspeed=+0.1;
						this->turnspeed=0.0;
						if((this->formationPosition==posterior)&&(!this->naviCheckDir(this->formationYaw)))
						{

							this->pushDr=true;
						}
						else
							this->pushDr=false;
						break;
					case 0:// correct position
						cout<<this->dalekName<<" formation Keeper: correct position!"<<endl;
						this->stopInCorrectPosition();
						break;
					case -1:// indietro
						cout<<this->dalekName<<" formation keeper: indietro!"<<endl;
						this->pushDr=false;
						this->forwardspeed=-0.1;
						this->turnspeed=0.0;
						break;
					}
					//cout<<this->dalekName<<" end formation keeper! <<<<<<<<<<<<<<<<<<<"<<endl;

				}
				else
					this->forwardspeed=0;


			}
			/*while(obtsFlag)
			{
				cout<<this->dalekName<<" loop OBTFLAG=true"<<endl;
				this->robotClient->Read();
				obtsFlag=this->checkObstacles();
				if(obstacleCount==10)
				{
					break;
				}
				obstacleCount++;
			}*/
			//cout<<this->dalekName<<" fspeed: "<<this->forwardspeed<<" tspeed: "<<this->turnspeed<<endl; usleep(500000);
			if(this->obstaclesPosition.can+this->obstaclesPosition.sec+this->obstaclesPosition.jast+this->obstaclesPosition.thay>=3)
			{
				this->obstaclesPosition=this->initObstaclesPosition(this->obstaclesPosition);
			}
			this->robotClient->Read();
			obtsFlag=this->checkObstacles();
			obstacleCount=0;
			this->p2dProxy->SetSpeed(this->forwardspeed,dtor(this->turnspeed));
			sleep(1);
			/*
			//			this->pushDr=true;
			//			this->simProxy->GetPose2d((char*)"who",this->docCoord.x,this->docCoord.y,this->docCoord.yaw);
			//			this->simProxy->GetPose2d((char*)"tardis", this->cabCoord.x,this->cabCoord.y,this->cabCoord.yaw);
			//			this->simProxy->GetPose2d((char*)this->dalekName.c_str(), this->myCoord.x,this->myCoord.y,this->myCoord.yaw);
			//			this->checkRole();
			//
			//			if(this->obstaclesPosition.can+this->obstaclesPosition.sec+this->obstaclesPosition.jast+this->obstaclesPosition.thay>=3)
			//			{
			//				if (this->imNavigator)
			//				{
			//					cout<<"NAVIGATOR"<<endl;
			//					this->navigate();
			//
			//				}
			//				this->stopFlag=false;
			//				switch ((int)this->formationYaw)
			//				{
			//				case 180:
			//					if(this->obstaclesPosition._180)
			//					{
			//						this->stopFlag=true;
			//						//cout<<this->dalekName<<" ricevuto ostacolo a 180"<<endl;
			//					}
			//					break;
			//				case 0:
			//					if(this->obstaclesPosition._0)
			//					{
			//						this->stopFlag=true;
			//						//cout<<this->dalekName<<" ricevuto ostacolo a 0"<<endl;
			//					}
			//					break;
			//				case 270:
			//					if(this->obstaclesPosition._270)
			//					{
			//						this->stopFlag=true;
			//						//cout<<this->dalekName<<" ricevuto ostacolo a _270"<<endl;
			//					}
			//					break;
			//				case 90:
			//					if(this->obstaclesPosition._90)
			//					{
			//						this->stopFlag=true;
			//						//cout<<this->dalekName<<" ricevuto ostacolo a _90"<<endl;
			//					}
			//					break;
			//				}
			//
			//				//reset obstaclesPosition
			//				this->obstaclesPosition._0=false;
			//				this->obstaclesPosition._90=false;
			//				this->obstaclesPosition._180=false;
			//				this->obstaclesPosition._270=false;
			//				this->obstaclesPosition.can=0;
			//				this->obstaclesPosition.jast=0;
			//				this->obstaclesPosition.sec=0;
			//				this->obstaclesPosition.thay=0;
			//			}
			//			else
			//			{
			//				if(this->imNavigator)
			//					cout<< "NAVIGAZIONE impossibru"<<endl<<this->obstaclesPosition.can+this->obstaclesPosition.sec+this->obstaclesPosition.jast+this->obstaclesPosition.thay<<endl;
			//			}
			//
			//			if(this->state==FORMATIONGO){
			//				this->checkObstacles();//funzione booleana ritorna true se NON ci sono ostacoli, manda mess altrimenti e ret false
			//				correctAngle(&(this->turnspeed));
			//				if(this->stopFlag)
			//				{
			//					cout<<this->dalekName<< "wait!stopflag: "<<this->stopFlag<<endl;
			//					if(this->formationPosition==posterior)
			//					{
			//						this->iCanPush=false;
			//					}
			//					stopInCorrectPosition();
			//				}
			//				else{
			//					switch(formationKeeper())
			//					{
			//					case 1:
			//						if(this->formationPosition==posterior)
			//						{
			//							this->iCanPush=true;
			//						}
			//						this->forwardspeed=+0.3;
			//						this->turnspeed=0.0;
			//						break;
			//					case 0:
			//						if(this->formationPosition==posterior)
			//						{
			//							this->iCanPush=false;
			//						}
			//						cout<<this->dalekName<<" formationKeeper 0"<<endl;
			//						stopInCorrectPosition();
			//						break;
			//					case -1:
			//						if(this->formationPosition==posterior)
			//						{
			//							this->iCanPush=false;
			//						}
			//						this->forwardspeed=-0.1;
			//						this->turnspeed=0.0;
			//						break;
			//					}
			//				}
			//			}
			//			cout<<this->dalekName<<" fspeed: "<<this->forwardspeed<<" tspeed: "<<this->turnspeed<<endl;
			//			this->p2dProxy->SetSpeed(this->forwardspeed,dtor(this->turnspeed));
			//			usleep(125000);
			 *
			 */
			break;
		case PaskTest:
			this->simProxy->GetPose2d((char*)"who",this->docCoord.x,this->docCoord.y,this->docCoord.yaw);
			this->simProxy->GetPose2d((char*)"tardis", this->cabCoord.x,this->cabCoord.y,this->cabCoord.yaw);
			this->simProxy->GetPose2d((char*)this->dalekName.c_str(), this->myCoord.x,this->myCoord.y,this->myCoord.yaw);
			if(this->obstaclesPosition.can+this->obstaclesPosition.sec+this->obstaclesPosition.jast+this->obstaclesPosition.thay>=3)
			{
				if (this->imNavigator)
				{
					cout<<"==> NAVIGATOR <=="<<endl;
					contatoreNavigatore=0;
					cout<<"NAVIGATING!"<<endl;
					this->navigate();
				}

				//resetting obstaclesPosition
			}
			this->stopInCorrectPosition();
			if(this->obstaclesPosition.can+this->obstaclesPosition.sec+this->obstaclesPosition.jast+this->obstaclesPosition.thay>=3)
			{
				this->obstaclesPosition=this->initObstaclesPosition(this->obstaclesPosition);
			}
			this->robotClient->Read();
			obtsFlag=this->checkObstacles();
			obstacleCount=0;
			this->p2dProxy->SetSpeed(this->forwardspeed,dtor(this->turnspeed));
			sleep(1);
			break;
		default:
			break;
		}
		this->handleTarget(&foundDrFlag, &push);
		this->robotClient->Read();
	}
}

/*
void Dalek::changeState(states_t stateToChange)
{
	this->state=stateToChange;
	this->broadcastStateFunction(this->state);
	return;
}
 */

string Dalek::checkEveryoneState(states_t state1)
{
	//se tutti gli stati sono impostati sullo stesso stato ritorna true altrimenti appena ne trova uno diverso ret false
	for(list<otherstates_t>::iterator it = this->dalekNames.begin(); it !=  this->dalekNames.end(); it++)
	{
		if((*it).state!=state1)
		{
			cout<< this->dalekName<<" state of "<<(*it).name<< " is "<<this->stateToString((*it).state)<<endl;
			return (*it).name;
		}
	}
	return "OK";
}

bool Dalek::checkEveryoneState(states_t state1, states_t state2)
{
	//se tutti gli stati sono impostati sullo stesso stato ritorna true altrimenti appena ne trova uno diverso ret false
	for(list<otherstates_t>::iterator it = this->dalekNames.begin(); it !=  this->dalekNames.end(); it++)
	{
		if(((*it).state!=state1)&&((*it).state!=state2))
		{
			cout<< this->dalekName<<" state of "<<(*it).name<< " is "<<this->stateToString((*it).state)<<endl;
			return false;
		}
	}
	return true;
}

//questo metodo va chiamato in un ciclo o simulazione di questo...
//usare una flag dall'esterno che controlla il valore tornato dalla funzione
//se il valore della flag è false richiamare la funzione,
//se il valore è true si può passare all'operazione successiva!
bool Dalek::turnToYaw(double yaw,double errormargin)
{
	this->robotClient->Read();
	coordinates_t coordinateDalek;
	this->simProxy->GetPose2d((char*)this->dalekName.c_str(), coordinateDalek.x, coordinateDalek.y, coordinateDalek.yaw);
	coordinateDalek.yaw*=RADIUSCONSTANTCONVERSION;
	const double ERRORANGLEMARGIN=errormargin;
	const double HIGHESTMARGIN=15+errormargin;
	const double MEDIUMMARGIN=10+errormargin;
	double differenceYaw;
	bool returnFlag=false;

	if(this->counterYaw==0)
	{
		this->wantedYaw=coordinateDalek.yaw+yaw;
		this->counterYaw++;
		if(this->wantedYaw>180)
		{
			this->wantedYaw=(-180+(this->wantedYaw-180));
		}
		differenceYaw=this->wantedYaw-coordinateDalek.yaw;
	}
	else
	{
		differenceYaw=this->wantedYaw-coordinateDalek.yaw;
		if(differenceYaw>=0)
		{
			if(differenceYaw>HIGHESTMARGIN)
			{
				this->turnspeed=8;
			}
			else if((differenceYaw<HIGHESTMARGIN)&&(differenceYaw>MEDIUMMARGIN))
			{
				this->turnspeed=3;
			}
			else if((differenceYaw<MEDIUMMARGIN)&&(differenceYaw>ERRORANGLEMARGIN))
			{
				this->turnspeed=1;
			}
			else if(differenceYaw<=ERRORANGLEMARGIN)
			{
				this->turnspeed=0;
				this->counterYaw=0;
				returnFlag=true;
			}
		}else{
			if(fabs(differenceYaw)>HIGHESTMARGIN)
			{
				this->turnspeed=-8;
			}
			else if((fabs(differenceYaw)<HIGHESTMARGIN)&&(fabs(differenceYaw)>MEDIUMMARGIN))
			{
				this->turnspeed=-3;
			}
			else if((fabs(differenceYaw)<MEDIUMMARGIN)&&(fabs(differenceYaw)>ERRORANGLEMARGIN))
			{
				this->turnspeed=-1;
			}
			else if(fabs(differenceYaw)<=ERRORANGLEMARGIN)
			{
				this->turnspeed=0;
				this->counterYaw=0;
				returnFlag=true;
			}
		}
	}
	//cout<<"difference: "<<differenceYaw<<" myY: "<<coordinateDalek.yaw<<" yaw: "<<yaw<<" wantedY: "<<this->wantedYaw<<endl;
	this->forwardspeed=0.0;
	this->p2dProxy->SetSpeed(this->forwardspeed, dtor(this->turnspeed));
	usleep(250000);
	return returnFlag;
}

bool Dalek::turnToYawGlobalSafe(double yaw,double errormargin) //richiede sia stato fatto il getPose su myCoord
{
	const double ERRORANGLEMARGIN=errormargin-0.5;
	const double HIGHESTMARGIN=20+errormargin;
	const double MEDIUMMARGIN=15+errormargin;
	double differenceYaw;
	bool returnFlag=false;

	double targetYaw=yaw;
	if(targetYaw>180)
	{
		targetYaw=targetYaw-360;
	}
	if(targetYaw<-180)
	{
		targetYaw=targetYaw+360;
	}
	differenceYaw=targetYaw-this->myCoord.yaw*RADIUSCONSTANTCONVERSION;

	if(fabs(differenceYaw)>180)
	{
		if(differenceYaw>0)
			differenceYaw=differenceYaw-360;
		else
			differenceYaw=differenceYaw+360;

	}

	if(fabs(differenceYaw)>HIGHESTMARGIN)
	{
		this->turnspeed=5;
	}
	else if((fabs(differenceYaw)<HIGHESTMARGIN)&&(fabs(differenceYaw)>MEDIUMMARGIN))
	{
		this->turnspeed=2;
	}
	else if((fabs(differenceYaw)<MEDIUMMARGIN)&&(fabs(differenceYaw)>ERRORANGLEMARGIN))
	{
		this->turnspeed=0.5;
	}
	else if(fabs(differenceYaw)<ERRORANGLEMARGIN)
	{
		this->turnspeed=0;
		returnFlag=true;
	}

	if(differenceYaw<0)
		this->turnspeed= this->turnspeed * (-1);


	return returnFlag;
}

bool Dalek::noneIsAwain()
{
	for(list<otherstates_t>::iterator it = this->dalekNames.begin(); it !=  this->dalekNames.end(); it++)
	{
		cout<< this->dalekName<< " see: "<< (*it).name <<" with state: "<<this->stateToString((*it).state)<<endl;
		if(((*it).state==DOCFOUND)||((*it).state==SEENDOCTOR))
		{
			return false;
		}
	}
	return true;
}

bool Dalek::goAwayFromDoctor(double distanceneed)
{
	const double DISTANCENEED=distanceneed;
	//girare di 180° e poi allontanarsi per la distanza necessaria a far spazio al dalek che sta girando
	double distance=0.0;
	this->robotClient->Read();
	this->simProxy->GetPose2d((char*)"who",this->docCoord.x,this->docCoord.y,this->docCoord.yaw);
	this->simProxy->GetPose2d((char*)this->dalekName.c_str(),this->myCoord.x,this->myCoord.y,this->myCoord.yaw);
	distance=sqrt( pow( this->docCoord.x - this->myCoord.x , 2) + pow(this->docCoord.y - this->myCoord.y,2) );

	if(distance<=DISTANCENEED)// questo era while!!
	{
		this->robotClient->Read();
		if(this->allontanamentoCount==0)
		{
			//cout<<"180"<<endl;
			if(this->turnToYaw(180))
			{
				this->allontanamentoCount++;
			}
			//this->turnspeed=-180;//cambiare invocando un metodo
			this->forwardspeed=0.0;
		}
		else
		{
			//this->turnspeed=0.0;
			this->forwardspeed=0.2;
		}

		this->p2dProxy->SetSpeed(this->forwardspeed,dtor(this->turnspeed));
		usleep(125000);
		return false;
	}
	else if(distance>DISTANCENEED)
	{
		return true;
	}
	return false;
}

bool Dalek::isInTheAround(coordinates_t toCheck, coordinates_t theAround)
{
	const int AROUNDRAY=0.3;
	const int AROUNDX=theAround.x;
	const int AROUNDY=theAround.y;
	const int POINTX=toCheck.x;
	const int POINTY=toCheck.y;
	if((POINTX==AROUNDX)&&(POINTY==AROUNDY))
	{
		return true;
	}
	if((POINTX<AROUNDX+AROUNDRAY)&&(POINTX>AROUNDX-AROUNDRAY))
	{
		if((POINTY<AROUNDY+AROUNDRAY)&&(POINTY>AROUNDY-AROUNDRAY))
		{
			return true;
		}
	}
	return false;
}

bool Dalek::areSensorsLeftFree(double frontsx,double frontdx,double lateral,double anterior,double diagonal)
{
	const double OBSTACLEFRONTSXDISTANCE=frontsx; //una soglia differente per ogni sonar
	const double OBSTACLEFRONTDXDISTANCE=frontdx;
	const double OBSTACLELATERALDISTANCE=lateral;
	const double OBSTACLEANTERIORDISTANCE=anterior;
	const double OBSTACLEDIAGONALDISTANCE=diagonal;

	//prendiamo solo i sx perchè stiamo effettuando un movimento rotatorio orario
	double toothsx=this->sonarProxy->GetScan(0);     //dente sx
	double toothdx=this->sonarProxy->GetScan(1);     //dente dx
	double anteriorsx=this->sonarProxy->GetScan(4);  //anteriore sx
	double lateralsx=this->sonarProxy->GetScan(6);   //laterale sx
	double diagonalsx=this->sonarProxy->GetScan(7);  //diagonale sx

	/*cout << "toothsx: "<< toothsx<<endl;
	 */

	if(toothdx<OBSTACLEFRONTDXDISTANCE)
	{
		//cout <<this->dalekName<< " obstacle toothsx: "<< toothsx<<endl;
		return false;
	}
	if(toothsx<OBSTACLEFRONTSXDISTANCE)
	{
		//cout<<this->dalekName << " obstacle toothdx: "<< toothdx<<endl;
		return false;
	}
	if(anteriorsx<OBSTACLEANTERIORDISTANCE)
	{
		//cout <<this->dalekName<< " obstacle anteriorsx: "<< anteriorsx<<endl;
		return false;
	}
	if(lateralsx<OBSTACLELATERALDISTANCE)
	{
		//cout <<this->dalekName<< " obstacle lateralsx: "<< lateralsx<<endl;
		return false;
	}
	if(diagonalsx<OBSTACLEDIAGONALDISTANCE)
	{
		//cout <<this->dalekName<< " obstacle diagonalsx: "<< diagonalsx<<endl;
		return false;
	}

	return true;
}

bool Dalek::circumnavigateDr()
{
	this->robotClient->Read();
	this->simProxy->GetPose2d((char*)this->dalekName.c_str(),this->myCoord.x,this->myCoord.y,this->myCoord.yaw);

	//double dist=sqrt(fabs(pow(this->docCoord.x - this->myCoord.x,2)-pow(this->docCoord.y - this->myCoord.y,2)));

	if(this->circumnavigateCount==0)
	{
		if(this->areSensorsLeftFree(0,0,0.6,0.6,0.6))
		{
			if(!this->turnToYaw(90))
			{
				this->circumnavigateCount--;
			}
			else
			{
				this->startingCoord.x=this->myCoord.x;
				this->startingCoord.y=this->myCoord.y;
				//cout<<"starting x: "<<this->startingCoord.x<<endl;
				//cout<<"starting y: "<<this->startingCoord.y<<endl;
			}
		}
		else
		{
			cout<<"problema ostacolo durante la rotazione!! arrestare esecuzione?"<<endl;
			this->forwardspeed=0;
			this->turnspeed=0;
			this->writeOnPipe(goToStop,SEND_TO_ALL,0.0,0.0);
			return false;
		}
	}
	else
	{
		if(this->areSensorsLeftFree()){
			//un pezzotto di moto circolare uniforme
			//da affinare con i sensori destri
			this->forwardspeed=0.2;
			this->turnspeed=-7;
			//cout<<"mycoord x: "<<this->myCoord.x<<endl;
			//cout<<"mycoord y: "<<this->myCoord.y<<endl;

			if((this->circumnavigateCount>20) &&
					(this->isInTheAround(this->myCoord,this->startingCoord)))
			{
				/*cout<<"che ano!"<<endl;
				cout<<"starting x: "<<this->startingCoord.x<<endl;
				cout<<"starting y: "<<this->startingCoord.y<<endl;
				cout<<"mycoord x: "<<this->myCoord.x<<endl;
				cout<<"mycoord y: "<<this->myCoord.y<<endl;*/
				this->forwardspeed=0;
				this->turnspeed=0;
				this->writeOnPipe(drIsFree,SEND_TO_ALL,0,0);
				this->state=FORMATIONFARAWAY;
				this->broadcastStateFunction(this->state);
			}
		}
		else
		{
			cout<<"problema ostacolo!! arrestare esecuzione?"<<endl;
			this->forwardspeed=0;
			this->turnspeed=0;
			this->writeOnPipe(goToStop,SEND_TO_ALL,0.0,0.0);
			return false;
			//this->moveDr();
			//caso in cui si è incontrato un ostacolo!
			//arrestare esecuzione, oppure spingere il dr ???
		}
	}
	this->p2dProxy->SetSpeed(this->forwardspeed, dtor(this->turnspeed));
	usleep(125000);

	return true;
}

void Dalek::moveDr()
{
	//simulazione di fisica per spostare il dottore
	this->turnspeed=-90;
	this->forwardspeed=0.2;
}

void Dalek::checkCabin()
{
	this->robotClient->Read();
	const double MINDISTANCEROBOTCAB=2.5;
	double distance=0.0;
	this->simProxy->GetPose2d((char*)"tardis",this->cabCoord.x,this->cabCoord.y,this->cabCoord.yaw);
	this->simProxy->GetPose2d((char*)this->dalekName.c_str(),this->myCoord.x,this->myCoord.y,this->myCoord.yaw);
	distance=sqrt( pow( this->cabCoord.x - this->myCoord.x , 2) + pow(this->cabCoord.y - this->myCoord.y,2) );

	//cout << "cabin distance: "<< distance<<endl;
	if(distance>MINDISTANCEROBOTCAB)
	{
		this->forwardspeed=0.2;
		this->avoidObstaclesWithTarget();
		if(!this->avoiding)
		{
			this->moveToItem(PINK);
		}
	}
	else
	{
		this->forwardspeed=0.0;
		this->turnspeed=0.0;
		this->state=IFOUNDCAB;
		this->broadcastStateFunction(this->state);
	}
	this->p2dProxy->SetSpeed(this->forwardspeed,dtor(this->turnspeed));
}

void Dalek::checkDoctor()
{
	const double MAXDISTANCEROBOTDR=1.6; //la distanza tra i centri precisa è 1.025
	const double MEDIUMDISTANCEROBOTDR=2.0; //distanza di avvicinamento dopo la quale bisogna rallentare
	const double MINDISTANCEROBOTDR=1.45; //soglia dopo la quale bisogna fare retromarcia
	double distance=0;
	this->robotClient->Read();
	this->simProxy->GetPose2d((char*)"who",this->docCoord.x,this->docCoord.y,this->docCoord.yaw);
	this->simProxy->GetPose2d((char*)this->dalekName.c_str(),this->myCoord.x,this->myCoord.y,this->myCoord.yaw);
	distance=sqrt( pow( this->docCoord.x - this->myCoord.x , 2) + pow(this->docCoord.y - this->myCoord.y,2) );

	//cout<<this->dalekName<<" dr distance: "<<distance<<endl;
	/*"\nfrom: docX: "<<this->docCoord.x <<
			" - myX: "<<this->myCoord.x<<
			"\nand docY: "<<this->docCoord.y <<
			" - myY: "<<this->myCoord.y<<endl;*/

	if (distance>=MEDIUMDISTANCEROBOTDR)
	{
		this->forwardspeed=0.2;
		this->avoidObstaclesWithTarget();
		if(!this->avoiding)
		{
			this->moveToItem(ORANGE);
		}
	}
	else if((distance>MAXDISTANCEROBOTDR)&&(distance<MEDIUMDISTANCEROBOTDR))
	{
		this->forwardspeed=0.05;
		this->avoidObstaclesWithTarget();
		if(!this->avoiding)
		{
			this->moveToItem(ORANGE);
		}
	}
	else if(distance<=MINDISTANCEROBOTDR)
	{
		this->forwardspeed=-0.05;
	}
	else
	{
		this->forwardspeed=0.0;
		this->turnspeed=0.0;
		this->state=IFOUNDDR;
		this->broadcastStateFunction(this->state);
	}

	//this->reachLocation(this->docCoord.x, this->docCoord.y, *this->simProxy,0, &this->forwardspeed, &this->turnspeed);
	//this->avoidObstacles(&this->forwardspeed, &this->turnspeed, *this->sonarProxy, false);
	this->p2dProxy->SetSpeed(this->forwardspeed,dtor(this->turnspeed));

	usleep(250000);
}

void Dalek::wanderingState(bool foundDrFlag,uint32_t targetcolor,bool push,double temperature)
{
	this->robotClient->Read();

	this->handleTarget(&foundDrFlag, &push);
	// read from the proxies
	this->robotClient->Read();
	//wander
	if(!countColorBlob(targetcolor,*(this->blobProxy)))
	{
		//wander
		this->wander();
		this->avoidObstacles(&this->forwardspeed, &this->turnspeed, *this->sonarProxy, push);
	}
	else
	{
		if( this->blobFound.color == ORANGE)
		{
			this->simProxy->GetPose2d((char*)"who",this->docCoord.x,this->docCoord.y,this->docCoord.yaw);
			this->state=SEENDOCTOR;
			this->broadcastStateFunction(this->state);
		}
		else if( this->blobFound.color == PINK )
		{
			this->simProxy->GetPose2d((char*)"tardis",this->cabCoord.x,this->cabCoord.y,this->cabCoord.yaw);
			this->state=SEENCABIN;
			this->broadcastStateFunction(this->state);
		}
		//move towards the item
		//this->moveToItem(&forwardSpeed, &turnSpeed, *this->blobProxy, targetcolor);
		//move towards the item
		//this->moveToItem(&forwardSpeed, &turnSpeed, *this->blobProxy, targetcolor);
	}
	//set motors
	this->p2dProxy->SetSpeed(this->forwardspeed, dtor(this->turnspeed));
	usleep(125000);
}

void Dalek::avoidObstaclesWithTarget()
{
	//will avoid obstacles closer than 40cm
	double avoidDistanceTooth = 1;
	double avoidDistanceCorner = 1.5;
	double avoidFrontLateralImpDistance=0.4;
	double avoidLateralImpDistance=0.4;
	double noTurnDist=0.25;
	double moveBackDist=0.3;
	//will turn away at 60 degrees/sec

	int harshturn=60;
	int mincorrectionstep=2;
	int minSpeedReduct=0.05;
	int impactturn= 10-rand()%20;
	double avoidImpactSpeed=0.01*(5-rand()%10);
	double stopSpeed=0;



	bool canTurn=true;

	for (int i=2;i<10;i++) //tenere coerente con dalek.inc, 0 e 1 sono i tooth e vanno esclusi
	{
		if(this->sonarProxy->GetScan(i)<noTurnDist)
		{
			canTurn=false;
			//this->avoiding=true;
		}
	}
	if (canTurn)
	{
		bool frontObstacle=false;
		for (int i=0;i<2;i++) //tenere coerente con dalek.inc, 0 e 1 sono i tooth e vanno esclusi
		{
			if(this->sonarProxy->GetScan(i)<avoidDistanceTooth)
			{
				frontObstacle=true;
			}
		}
		for (int i=3;i<6;i++) //tenere coerente con dalek.inc, 0 e 1 sono i tooth e vanno esclusi
		{
			if(this->sonarProxy->GetScan(i)<avoidDistanceCorner)
				frontObstacle=true;
		}
		if (frontObstacle)
		{
			this->avoiding=true;
			int direzione;
			if (this->sonarProxy->GetScan(2)<this->sonarProxy->GetScan(3))
				direzione=-1;
			else
				direzione=1;
			this->forwardspeed=stopSpeed;
			if((this->sonarProxy->GetScan(0)<=0.1)||(this->sonarProxy->GetScan(1)<=0.1))
				this->forwardspeed=-0.05;
			this->turnspeed=direzione*mincorrectionstep;
			if(rand()%6==0)
				this->turnspeed=direzione*harshturn;



			//std::cout << this->dalekName <<" evita ostacolo! direzione " << direzione*turnalittle<<"sp2:"<<sp[2]<<" sp3:"<<sp[3]<<std::endl;
			return;
		}


		//sensor code sensori laterali
		int latSensorCode=0;

		if(this->sonarProxy->GetScan(6) > avoidLateralImpDistance)
		{
			latSensorCode=1000;
		}
		if(this->sonarProxy->GetScan(7) > avoidFrontLateralImpDistance)
		{
			latSensorCode+=100;
		}
		if(this->sonarProxy->GetScan(8) > avoidFrontLateralImpDistance)
		{
			latSensorCode+=10;
		}
		if(this->sonarProxy->GetScan(9) > avoidLateralImpDistance)
		{
			latSensorCode+=1;
		}





		//questo comportamento è per il robot da solo
		//se diventa un TEAM bisogna cambiarlo
		//deve essere dominante il sensorcode, questi servono solo come aggiustamenti di direzione
		switch (latSensorCode){
		case 110:
			this->forwardspeed=this->forwardspeed-minSpeedReduct;
			this->turnspeed= this->turnspeed/2;
			this->avoiding=true;
			break;//continua dritto
		case 101:
		case 100:
		case 1101:
		case 1100:
		case 1110:
			this->forwardspeed=this->forwardspeed-minSpeedReduct;
			this->turnspeed= this->turnspeed+mincorrectionstep;
			this->avoiding=true;
			break;//gira poco a sinistra
		case 10:
		case 1010:
		case 111:
		case 1011:
		case 11:
			this->forwardspeed=this->forwardspeed-minSpeedReduct;
			this->turnspeed= this->turnspeed-mincorrectionstep;
			this->avoiding=true;
			break;//gira poco a destra
		case 1111:
		case 0:
		case 1:
		case 1000:
		case 1001:
		default:
			//cout<<"first false avoiding"<<endl;
			this->avoiding=false;
			break;
		}

		//std::cout << this->dalekName <<" sensor code: "<< sensorCode  << " + lateral + " << latSensorCode << std::endl << "avoid fspeed: " << *forwardSpeed << "tspeed: "<< *turnSpeed << std::endl;
		return; //do nothing

	}
	else //rotazione ostacolata
	{
		bool frontObstacle=false;
		for (int i=0;i<2;i++) //tenere coerente con dalek.inc, 0 e 1 sono i tooth e vanno esclusi
		{
			if(this->sonarProxy->GetScan(i)<avoidDistanceTooth)
			{
				frontObstacle=true;
				this->avoiding=true;
			}
		}
		for (int i=3;i<6;i++) //tenere coerente con dalek.inc, 0 e 1 sono i tooth e vanno esclusi
		{
			if(this->sonarProxy->GetScan(i)<avoidDistanceCorner)
			{
				frontObstacle=true;
				this->avoiding=true;
			}

		}
		if(!frontObstacle)
		{
			return;
		}

		if ((this->sonarProxy->GetScan(10)> moveBackDist)&& (this->sonarProxy->GetScan(11)>moveBackDist))
		{
			this->forwardspeed=-0.2;


		}
		else
		{
			this->forwardspeed=avoidImpactSpeed;
			this->turnspeed=impactturn;
		}
		return;
	}




}

void Dalek::avoidObstacles(double *forwardSpeed, double *turnSpeed, SonarProxy &sp , bool pushing)
{
	//will avoid obstacles closer than 40cm
	double avoidDistanceTooth = 1;
	double avoidToothImpactDistance=0.2;
	double avoidDistanceCorner = 1.5;
	double avoidSideImpactDistance=0.5;
	double avoidFrontLateralImpDistance=0.4;
	double avoidLateralImpDistance=0.4;
	double noTurnDist=2.75;
	double moveBackDist=0.3;
	//will turn away at 60 degrees/sec

	int backroll=180;
	int harshturn=60;
	int turnalot=30;
	int turnalittle=10;
	int mincorrectionstep=10;
	int minSpeedReduct=0.05;
	int impactturn= 10-rand()%20;
	double avoidImpactSpeed=0.01*(5-rand()%10);
	double stopSpeed=0;

	if (pushing)
	{
		avoidDistanceTooth = 2.5;
		avoidToothImpactDistance=1.7;
		avoidDistanceCorner = 2;
		avoidSideImpactDistance=1;
		avoidFrontLateralImpDistance=2;
		avoidLateralImpDistance=2;
		//will turn away at 60 degrees/sec

		backroll=180;
		harshturn=60;
		turnalot=30;
		turnalittle=10;
		impactturn= 10-rand()%20;
		avoidImpactSpeed=0.01*(5-rand()%10);
		stopSpeed=0;

	}
	bool canTurn=true;

	for (int i=2;i<10;i++) //tenere coerente con dalek.inc, 0 e 1 sono i tooth e vanno esclusi
	{
		if(sp[i]<noTurnDist)
			canTurn=false;
	}
	if (canTurn)
	{
		bool frontObstacle=false;
		for (int i=0;i<2;i++)
		{
			if(sp[i]<avoidDistanceTooth)
				frontObstacle=true;
		}
		for (int i=3;i<6;i++) //tenere coerente con dalek.inc, 0 e 1 sono i tooth e vanno esclusi
		{
			if(sp[i]<avoidDistanceCorner)
				frontObstacle=true;
		}
		if (frontObstacle)
		{
			int direzione;
			if (sp[2]<sp[3])
				direzione=-1;
			else
				direzione=1;


			*forwardSpeed=stopSpeed;
			//cout << this->dalekName << "avoidStopped! mincorr: "<< mincorrectionstep << endl;
			if((sp[0]<=0.1)||(sp[1]<=0.1))
				*forwardSpeed=-0.05;
			*turnSpeed=direzione*mincorrectionstep;
			if(rand()%6==0)
				*turnSpeed=direzione*harshturn;


			//std::cout << this->dalekName <<" evita ostacolo! direzione " << direzione*turnalittle<<"sp2:"<<sp[2]<<" sp3:"<<sp[3]<<std::endl;
			return;
		}

		/*		int sensorCode=0;
		//sensor code sensori interni 1-4

		if(sp[2] > avoidDistanceCorner)
		{
			sensorCode=1000;
		}
		if(sp[3] > avoidDistanceCorner)
		{
			sensorCode+=1;
		}
		if(sp[0] > avoidDistanceTooth)
		{
			sensorCode+=100;
		}
		if(sp[1] > avoidDistanceTooth)
		{
			sensorCode+=10;
		}
		 */
		//sensor code sensori laterali
		int latSensorCode=0;

		if(sp[6] > avoidLateralImpDistance)
		{
			latSensorCode=1000;
		}
		if(sp[7] > avoidFrontLateralImpDistance)
		{
			latSensorCode+=100;
		}
		if(sp[8] > avoidFrontLateralImpDistance)
		{
			latSensorCode+=10;
		}
		if(sp[9] > avoidLateralImpDistance)
		{
			latSensorCode+=1;
		}


		/*		//early avoidance
		switch (sensorCode){
		case 1111:
			break;
		case 110:
		 *turnSpeed= *turnSpeed/5;
			break;//continua dritto
		case 0:
		case 100:
		case 10:
		 *forwardSpeed=avoidImpactSpeed;
			if(rand()%2==0)
		 *turnSpeed= backroll;
			else
		 *turnSpeed= (-1)*backroll;

			break;//gira a 180 gradi
		case 1:
		 *forwardSpeed=stopSpeed;
		 *turnSpeed= (-1)*harshturn;
			break;//gira di 90° a dex
		case 1000:

		 *forwardSpeed=stopSpeed;
		 *turnSpeed= harshturn;
			break;//gira di 90 a sin
		case 1001:
		 *forwardSpeed=stopSpeed;
			if(rand()%2==0)
		 *turnSpeed= harshturn;
			else
		 *turnSpeed= (-1)*harshturn;
			break;//gira di 90 in una dir. casuale
		case 111:
		 *forwardSpeed=(*forwardSpeed )/2;
		 *turnSpeed= (-1)*turnalittle;
			break;//gira di poco a dx

		case 1011:
		case 101:
		case 11:
		 *forwardSpeed=stopSpeed;
		 *turnSpeed= (-1)*turnalot;
			break;//gira di molto a dex
		case 1101:
		case 1010:
		case 1100:
		 *forwardSpeed=stopSpeed;
		 *turnSpeed= turnalot;
			break;//gira di molto a sin
		case 1110:
		 *forwardSpeed=avoidImpactSpeed;
		 *turnSpeed=impactturn;
			break;//gira di poco a sin
		default:
			break;
		}
		//questo comportamento è per il robot da solo
		//se diventa un TEAM bisogna cambiarlo
		//deve essere dominante il sensorcode, questi servono solo come aggiustamenti di direzione
		 */
		switch (latSensorCode){
		case 1111:
			break;
		case 110:
			*forwardSpeed=*forwardSpeed-minSpeedReduct;
			*turnSpeed= *turnSpeed/2;
			break;//continua dritto
		case 101:
		case 100:
		case 1101:
		case 1100:
		case 1110:
			*forwardSpeed=*forwardSpeed-minSpeedReduct;
			*turnSpeed= *turnSpeed+mincorrectionstep;
			break;//gira poco a sinistra
		case 10:
		case 1010:
		case 111:
		case 1011:
		case 11:
			*forwardSpeed=*forwardSpeed-minSpeedReduct;
			*turnSpeed= *turnSpeed-mincorrectionstep;
			break;//gira poco a destra
		case 0:
		case 1:
		case 1000:
		case 1001:
		default:
			break;
		}
		//impact handling
		/* if(sp[0]<avoidToothImpactDistance || sp[1]<avoidToothImpactDistance)
		{
		 *forwardSpeed=avoidImpactSpeed;
		 *turnSpeed=impactturn;
		}
		if(sp[2]<avoidSideImpactDistance)
		{
		 *forwardSpeed=avoidImpactSpeed+0.05;
		 *turnSpeed=impactturn;
		}
		if(sp[3]<avoidSideImpactDistance)
		{
		 *forwardSpeed=avoidImpactSpeed+0.05;
		 *turnSpeed=impactturn;
		}
		if(sp[4]<avoidToothImpactDistance)
		{
		 *forwardSpeed=avoidImpactSpeed-0.1;
		 *turnSpeed=impactturn-mincorrectionstep;
		}
		if(sp[5]<avoidToothImpactDistance)
		{
		 *forwardSpeed=avoidImpactSpeed-0.1;
		 *turnSpeed=impactturn+mincorrectionstep;
		}
		 */

		//std::cout << this->dalekName <<" sensor code: "<< sensorCode  << " + lateral + " << latSensorCode << std::endl << "avoid fspeed: " << *forwardSpeed << "tspeed: "<< *turnSpeed << std::endl;
		return; //do nothing

	}
	else //rotazione ostacolata
	{
		bool frontObstacle=false;
		for (int i=0;i<2;i++) //tenere coerente con dalek.inc, 0 e 1 sono i tooth e vanno esclusi
		{
			if(sp[i]<avoidDistanceTooth)
				frontObstacle=true;
		}
		for (int i=3;i<6;i++) //tenere coerente con dalek.inc, 0 e 1 sono i tooth e vanno esclusi
		{
			if(sp[i]<avoidDistanceCorner)
				frontObstacle=true;
		}
		if(!frontObstacle){
			return;
		}

		if ((sp[10]> moveBackDist)&& (sp[11]>moveBackDist))
		{
			*forwardSpeed=-0.2;

		}
		else
		{
			*forwardSpeed=avoidImpactSpeed;
			*turnSpeed=impactturn;
		}
		return;
	}
}

bool Dalek::watchAround(uint32_t targetcolor)
{
	bool checking=true;
	bool seen=false;
	int count=0;
	const int MAXCOUNTROUND=60;
	this->forwardspeed=0.0;
	this->turnspeed=15;
	int countRound=0;
	while(checking)
	{
		count=this->blobProxy->GetCount();
		if(count==0)
		{
			countRound++;
			this->p2dProxy->SetSpeed(0.0,dtor(this->turnspeed));
			if(countRound==MAXCOUNTROUND)
			{
				checking=false;
				seen=false;
			}
			usleep(125000);
		}
		else
		{
			for(int i=0; i<count; i++)
			{
				playerc_blobfinder_blob_t currBlob = this->blobProxy->GetBlob(i);
				if(currBlob.color==targetcolor)
				{
					this->turnspeed=0.0;
					this->p2dProxy->SetSpeed(this->forwardspeed,dtor(this->turnspeed));
					checking=false;
					seen=true;
				}
			}
		}
		this->robotClient->Read();
	}
	return seen;
}

void Dalek::moveToItem(uint32_t targetcolor)
{
	uint32_t centre;
	int i;
	const int MAXAROUNDCOUNT=5;

	int noBlobs = this->blobProxy->GetCount();
	if (noBlobs==0)
	{
		if(this->aroundCount==MAXAROUNDCOUNT)
		{
			if(!this->watchAround(targetcolor))
			{
				this->state=WANDERING;
				this->broadcastStateFunction(this->state);
			}
			this->aroundCount=0;
		}
		this->forwardspeed=0.2;
		this->turnspeed=0.0;
		this->aroundCount++;
	}
	else
	{
		playerc_blobfinder_blob_t blob;
		int turningSpeed = 5;
		/*number of pixels away from the image centre a blob
		can be to be in front of the robot*/
		uint32_t margin = 30;
		uint32_t biggestBlobArea = 0;
		int biggestBlob = 0;
		//find the largest blob
		for(i=0; i<noBlobs; i++)
		{
			//get blob from proxy
			playerc_blobfinder_blob_t currBlob = this->blobProxy->GetBlob(i);
			if(currBlob.color==targetcolor)
			{
				if(currBlob.area > biggestBlobArea)
				{
					biggestBlob = i;
					biggestBlobArea = currBlob.area;
				}
			}
		}
		blob = this->blobProxy->GetBlob(biggestBlob);
		// find centre of image
		centre = this->blobProxy->GetWidth()/2;
		//adjust turn to centre the blob in image
		/*if the blob’s centre is within some margin of the image
		centre then move forwards, otherwise turn so that it is
		centred. */
		//blob to the left of centre
		//if(this->movetoCounter<rotationtimes)
		//{
		if(blob.x < centre-margin)
		{
			this->forwardspeed = 0;
			//turn left
			this->turnspeed = turningSpeed;
		}
		//blob to the right of centre
		else if(blob.x > centre+margin)
		{
			this->forwardspeed = 0;
			//turn right
			this->turnspeed = -turningSpeed;
		}
		//otherwise go straight ahead
		else
		{
			this->forwardspeed = 0.2;
			this->turnspeed = 0;
		}
		//}
	}
	//cout << "FS: "<<this->forwardspeed <<" TS: "<<this->turnspeed<<endl;
	return;
}

void Dalek::wander()
{
	const double MAXSPEED = 0.49;
	const int MAXTURN = 60;
	const double ACCELERATION=0.15;
	const int TURNPROB=3;//if(rand()%turnprob==0)->turn else tspeed=0;
	double fspeed, tspeed;


	//srand(time(NULL));
	//fspeed is between 0 and 10
	this->forwardspeed+=ACCELERATION;
	if(this->forwardspeed>MAXSPEED)
	{
		fspeed = MAXSPEED;
	}else{
		fspeed =this->forwardspeed ;
	}
	//(fspeed/10) is between 0 and 1
	if(rand()%TURNPROB==0)
	{
		tspeed = rand() % (2*MAXTURN);
		tspeed = tspeed-MAXTURN;
	}
	else
		tspeed=0;
	//tspeed is between -maxTurn and +maxTurn
	this->forwardspeed = fspeed;
	this->turnspeed = tspeed;
	return;
}

int Dalek::FindItem()
{
	/*
This function works by creating a search area just
in front of the robot’s teeth. The search circle is a
fixed distance in front of the robot, and has a
fixed radius.
This function finds objects within this search circle
and then deletes the closest one.
	 */
	//radius of the search circle
	double radius = 1;
	//The distance from the centre of the robot to
	//the centre of the search circle
	double distBotToCircle = 1;
	double robotX = this->myCoord.x, robotY=this->myCoord.y, robotYaw=this->myCoord.yaw;
	double circleX, circleY;
	/*now we find the centre of the search circle.
this is distBotToCircle metres from the robot’s origin
along its yaw*/
	/*horizontal offset from robot origin*/
	circleX = distBotToCircle*cos(robotYaw);

	/*vertical offset from robot origin*/
	circleY = distBotToCircle*sin(robotYaw);

	//find actual centre relative to simulation.
	circleX = robotX + circleX;
	circleY = robotY + circleY;

	/* to find which items are within this circle we
	 *find their Euclidian distance to the circle centre.
	 *Find the closest one and if it’s distance is smaller than
	 *the circle radius then return its index */
	double smallestDist;
	double x, y, dist;
	// get manhattan distance from circle centre to item
	x = circleX - this->itemList.x;
	y = circleY - this->itemList.y;
	//find euclidian distance from circle centre to item

	dist = (x*x) + (y*y);
	smallestDist = sqrt(dist);
	if(smallestDist > radius)
	{
		return -1;
	}
	return 1;
}

void Dalek::RefreshItemList()
{

	double dummy; //dummy variable, don’t need yaws.
	this->simProxy->GetPose2d((char*)(this->itemList.name.c_str()), this->itemList.x, this->itemList.y, dummy);
	return;
}

void Dalek::handleTarget(bool *impactFlag, bool *pushing) //la funzione dipende da oldformationyaw, pushdr e forwardspeed
{

	double x, y;
	const double fixedMove=0.4;
	const double minOtherDalekDist=0.95;
	this->RefreshItemList();
	int u=FindItem();
	int direction;

	//	if(FindItem()==1 && this->forwardspeed>=0)
	if((this->pushDr) && this->forwardspeed>=0)
	{
		if(u==1)
		{
			coordinates_t temp;
			x=this->itemList.x;
			y=this->itemList.y;
			if(fabs(this->oldFormationYaw)<10){
				x+=fixedMove;//direction=0;
			}

			if(fabs(this->oldFormationYaw-90)<10){
				y+=fixedMove;//direction=90;
			}
			if(fabs(this->oldFormationYaw-180)<10){
				x-=fixedMove;//direction=180;
			}
			if(fabs(this->oldFormationYaw-270)<10){
				y-=fixedMove;//direction=270;
			}


			for(list<otherstates_t>::iterator it = this->dalekNames.begin(); it !=  this->dalekNames.end(); it++)
			{
				if((*it).name!=this->dalekName)
				{
					this->simProxy->GetPose2d((char*)(*it).name.c_str(), temp.x,temp.y,temp.yaw);
					if((fabs(x-temp.x)<minOtherDalekDist)&&((fabs(y-temp.y)<minOtherDalekDist)))
					{
						x=this->itemList.x;
						y=this->itemList.y;
						it= this->dalekNames.end();
					}
				}
			}
			//			this->simProxy->GetPose2d((char*)this->dalekName.c_str(), this->myCoord.x,this->myCoord.y,this->myCoord.yaw);
			this->simProxy->SetPose2d((char*)this->itemList.name.c_str(),x,y,0);

			cout<<this->dalekName<<" is pushing!"<<endl;


			//			x = cos(robotYaw);
			//			y = sin(robotYaw);
			//			if (x>=0)
			//			{
			//				if (y>=0)
			//				{
			//					if(x>y)
			//						this->simProxy->SetPose2d((char*)this->itemList.name.c_str(),this->itemList.x+fixedMove,this->itemList.y,0);
			//					else
			//						this->simProxy->SetPose2d((char*)this->itemList.name.c_str(),this->itemList.x,this->itemList.y+fixedMove,0);
			//				}else
			//				{
			//					if(fabs(x)>fabs(y))
			//						this->simProxy->SetPose2d((char*)this->itemList.name.c_str(),this->itemList.x+fixedMove,this->itemList.y,0);
			//					else
			//						this->simProxy->SetPose2d((char*)this->itemList.name.c_str(),this->itemList.x,this->itemList.y-fixedMove,0);
			//
			//				}
			//			}else
			//			{
			//				if (y>=0)
			//				{
			//					if(fabs(x)>fabs(y))
			//						this->simProxy->SetPose2d((char*)this->itemList.name.c_str(),this->itemList.x-fixedMove,this->itemList.y,0);
			//					else
			//						this->simProxy->SetPose2d((char*)this->itemList.name.c_str(),this->itemList.x,this->itemList.y+fixedMove,0);
			//
			//				}else
			//				{
			//					if(fabs(x)>fabs(y))
			//						this->simProxy->SetPose2d((char*)this->itemList.name.c_str(),this->itemList.x-fixedMove,this->itemList.y,0);
			//					else
			//						this->simProxy->SetPose2d((char*)this->itemList.name.c_str(),this->itemList.x,this->itemList.y-fixedMove,0);
			//
			//				}
			//			}

			*impactFlag=true;
			*pushing=true;
		}
	}
}

bool Dalek::countColorBlob(uint32_t color,BlobfinderProxy &bfp)
{
	int noBlobs = bfp.GetCount();
	bool result = false;
	//find the largest blob
	for(int i=0; i<noBlobs; i++)
	{
		//get blob from proxy
		playerc_blobfinder_blob_t currBlob = bfp[i];
		//cout<<"--------------->"<<currBlob.color<<endl;
		if(currBlob.color==color)
		{
			this->blobFound=currBlob;
			this->forwardspeed=0.0;
			this->turnspeed=0.0;
			result=true;
		}
	}
	return result;
}

void Dalek::reachLocation(double targetX, double targetY, SimulationProxy &simProxy,double temperature, double *forwardSpeed, double *turnSpeed)
{
	double absYaw;
	int counterlimit=5;
	int rotationtimes;
	if(fabs(temperature)<0.7)
		rotationtimes=counterlimit;
	else
		rotationtimes=1;
	const double maxspeed=0.2;
	if(this->reachlocationCounter>counterlimit)
	{
		this->reachlocationCounter=-1;
	}
	this->reachlocationCounter++;
	absYaw=angleDiff(this->myCoord.x,this->myCoord.y,this->myCoord.yaw, targetX,targetY);
	//absYaw ora contiene l'angolo di cui bisogna ruotare per puntare al target.


	//	if((fabs(absYaw)>10)&&(	this->reachlocationCounter<rotationtimes))
	if(	this->reachlocationCounter<rotationtimes)
	{
		*turnSpeed=getRandomRotationSpeed(-25, 50);
		cout<<this->dalekName<<"random: "<<*turnSpeed<<endl;
		*turnSpeed=(1-temperature)*(absYaw/4)+temperature*(*turnSpeed);
		cout<<this->dalekName<<*turnSpeed<<"=(1-"<<temperature<<")*("<<absYaw/4<<")+"<<temperature<<"*(vedi random);"<<endl;

		if(fabs(*turnSpeed)>5)
			*forwardSpeed=0;
		else
			*turnSpeed=0;
	}
	else{
		const double dist=sqrt(((targetX-this->myCoord.x)*(targetX-this->myCoord.x))+((targetY-this->myCoord.y)*(targetY-this->myCoord.y)));
		if(dist>2)
			*forwardSpeed=maxspeed;
		else
			*forwardSpeed=0.05;
	}
}

double Dalek::getRandomRotationSpeed(int startFrom, int widthInterval)
{
	return(rand()%widthInterval)+startFrom;

}

void Dalek::reachFormationPlace(SimulationProxy &simProxy, double forwardSpeed, double turnSpeed)
{

	//	this->robotClient->Read();
	double otherX, otherY, otherYaw;
	const double maxTurnSpeed=5;
	const double turningspeed=10;
	const double maxForwardSpeed=0.1;
	const double rotationDist=this->drDistance;
	const double rotationSpeed=0.1;
	const double margin= 0.5;
	double absYaw;
	double otherdist,robotdist;
	const double turnAngle=-85;//giro completo in 10 passaggi
	this->tempcounter++;
	if (this->tempcounter==11)
		this->tempcounter=0;
	const double placedist=sqrt(((this->formationPlace.x-this->myCoord.x)*(this->formationPlace.x-this->myCoord.x))+((this->formationPlace.y-this->myCoord.y)*(this->formationPlace.y-this->myCoord.y)));
	const double dist=sqrt(((this->docCoord.x-this->myCoord.x)*(this->docCoord.x-this->myCoord.x))+((this->docCoord.y-this->myCoord.y)*(this->docCoord.y-this->myCoord.y)));
	bool pathClear,allWaiting;
	cout<<this->dalekName<<" obiettivo ("<<this->formationPlace.x<<","<<this->formationPlace.y<<"), placedist:"<<placedist<<endl;
	if(placedist>0.5)
		pathClear=checkClearPath(rotationDist,margin);//TODO mi sa che invece di calcolare placedist sul formplace ci vuole il nextformplace...
	else
		pathClear=true;
	if(pathClear)
	{
		cout<<this->dalekName<<" clear!"<<endl;

		if(dist>rotationDist+margin)//caso avvicinamento da lunga distanza
		{

			if(this->state!=FORMATIONFARAWAY)
			{
				this->state=FORMATIONFARAWAY;
				broadcastStateFunction(this->state);
			}
			for(list<otherstates_t>::iterator it = this->dalekNames.begin(); it !=  this->dalekNames.end(); it++)
			{
				if((*it).inExpQuadrant==true)
				{
					cout<<endl;
					cout<< this->dalekName <<" versus "<< (*it).name;
					switch((*it).state)
					{
					case WANDERING: cout << "WANDERING "<<endl;
					break;
					case FORMATION:
					case FORMATIONFARAWAY:
						this->simProxy->GetPose2d((char*)(*it).name.c_str(), otherX, otherY, otherYaw);
						robotdist=sqrt(((this->docCoord.x-this->myCoord.x)*(this->docCoord.x-this->myCoord.x))+((this->docCoord.y-this->myCoord.y)*(this->docCoord.y-this->myCoord.y)));
						otherdist=sqrt(((this->docCoord.x-otherX)*(this->docCoord.x-otherX))+((this->docCoord.y-otherY)*(this->docCoord.y-otherY)));
						if(robotdist>otherdist)
						{
							cout << " FORMATION/FORMATIONFARAWAY-> a dormire! "<<endl;
							forwardSpeed=0; turnSpeed=0;
							this->p2dProxy->SetSpeed(forwardSpeed, dtor(turnSpeed));
							sleep(10);
							return;//aspettare
						}
						else
							cout << " FORMATION/WAITINFORMANSWER robo<=other"<<endl;

						break;
					case WAITINFORMANSWER: cout << " WAITINFORMANSWER "<<endl;
					forwardSpeed=0; turnSpeed=0;
					this->p2dProxy->SetSpeed(forwardSpeed, dtor(turnSpeed));
					sleep(10);
					return;//aspettare
					break;
					case WAITINFORMATION:
						cout << " WAITINFORMATION "<<endl;
						//gestire caso alpha
						//test: lascio fare ad avoid!
						break;
					case STOP: cout << " STOP"<<endl; break;
					case IFOUNDDR: cout << " IFOUNDDR"<<endl;break;
					case IFOUNDCAB:cout << " IFOUNDCAB"<<endl; break;
					case DOCFOUND: cout << " DOCFOUND"<<endl;break;
					case CABFOUND: cout << " CABFOUND"<<endl;break;
					case CHECKDRPOSITION: cout << " CHECKDRPOSITION"<<endl;break;
					default: cout << " default???"<<endl;break;
					}
				}
			}

			cout<< this->dalekName<< "comparison ends"<<endl;
			forwardSpeed=0.3;
			cout<<this->dalekName<<" °c: "<<this->tempcounter<<endl;
			this->reachLocation(this->docCoord.x, this->docCoord.y, *this->simProxy,((double) this->tempcounter)/10, &forwardSpeed, &turnSpeed);
			cout<<this->dalekName<<" vel: "<<forwardSpeed<<" rotaz: "<<turnSpeed<<endl;
			this->avoidObstacles(&forwardSpeed, &turnSpeed, *this->sonarProxy, false);
		}
		else
		{
			if(this->state!=FORMATION)
			{
				this->state=FORMATION;
				broadcastStateFunction(this->state);
			}
			for(list<otherstates_t>::iterator it = this->dalekNames.begin(); it !=  this->dalekNames.end(); it++)
			{
				if((*it).inExpQuadrant==true)
				{
					switch((*it).state)
					{
					case WANDERING:
					case FORMATIONFARAWAY:
						cout << " WANDERING/FORMATIONFARAWAY"<<endl;
						break;
					case FORMATION:
						//provo a far muovere comunque il robot, alla peggio lo tampina fino al formationplace
						//dopodichè l'altro va in waitinformation e gli verrà mandato shift
						//						this->simProxy->GetPose2d((char*)(*it).name.c_str(), otherX, otherY, otherYaw);
						//						robotdist=sqrt(((this->docCoord.x-this->myCoord.x)*(this->docCoord.x-this->myCoord.x))+((this->docCoord.y-this->myCoord.y)*(this->docCoord.y-this->myCoord.y)));
						//						otherdist=sqrt(((this->docCoord.x-otherX)*(this->docCoord.x-otherX))+((this->docCoord.y-otherY)*(this->docCoord.y-otherY)));
						//						if(robotdist>otherdist)
						//						{
						//							cout << " FORMATION "<<endl;
						//							forwardSpeed=0; turnSpeed=0;
						//							this->p2dProxy->SetSpeed(forwardSpeed, dtor(turnSpeed));
						//							sleep(10);
						//							return;//aspettare
						//						}
						//						else
						cout << " FORMATION robo<=other"<<endl;
						break;
					case WAITINFORMANSWER:
						cout << " WAITINFORMANSWER"<<endl;
						break;
					case WAITINFORMATION: cout << " WAITINFORMATION2"<<endl; break;
					case STOP: cout << " STOP2"<<endl; break;
					case IFOUNDDR: cout << " IFOUNDDR2"<<endl;break;
					case IFOUNDCAB:cout << " IFOUNDCAB2"<<endl; break;
					case DOCFOUND: cout << " DOCFOUND2"<<endl;break;
					case CABFOUND: cout << " CABFOUND2"<<endl;break;
					case CHECKDRPOSITION: cout << " CHECKDRPOSITION2"<<endl;break;
					default: cout << " default2???"<<endl;break;
					}
				}
			}

			//std::cout<<""<< std::endl;

			if(placedist<=0.8)//caso messa in posizione da brevissima distanza
			{
				if(placedist<=0.05)
				{
					allWaiting=true;
					for(list<otherstates_t>::iterator it = this->dalekNames.begin(); it !=  this->dalekNames.end(); it++)
					{
						if((WAITINFORMATION != (*it).state )&&((*it).name!=this->dalekName))
						{
							allWaiting=false;
						}
					}

					if(allWaiting)
					{
						this->imNavigator=true;
						this->state=FORMATIONGO;
						this->broadcastStateFunction(FORMATIONGO);
					}
					else
					{
						this->forwardspeed=0;
						this->turnspeed=0;
						this->state=WAITINFORMATION;
						broadcastStateFunction(this->state);
						std::cout<<this->dalekName <<" entra in WaitInFormation #####################"<< std::endl;
						this->p2dProxy->SetSpeed(0, dtor(0));
						return;
					}
				}
				else
				{
					cout<<this->dalekName<<" brevissima distanza!"<<endl;

					this->reachLocation(this->formationPlace.x, this->formationPlace.y, *this->simProxy,0, &forwardSpeed, &turnSpeed);

				}
			}
			else
			{
				absYaw=angleDiff(this->myCoord.x,this->myCoord.y,this->myCoord.yaw, this->docCoord.x,this->docCoord.y);

				//se in margine esegue step per rotaz, altrim. si allontana
				if(dist>=rotationDist-margin)
				{
					//caso: alla distanza giusta
					if(fabs(absYaw-turnAngle)<5)
					{
						forwardSpeed=rotationSpeed;
						turnSpeed=0;
					}
					else
					{
						cout<<this->dalekName<<"ora fa il turntoyawglobalsafe di "<<this->myCoord.yaw*RADIUSCONSTANTCONVERSION+absYaw-turnAngle<<endl;
						this->turnToYawGlobalSafe(this->myCoord.yaw*RADIUSCONSTANTCONVERSION+absYaw-turnAngle,5);
						this->p2dProxy->SetSpeed(0, dtor(this->turnspeed));
						return;
					}
				}
				else
				{//caso allontanarsi

					if(fabs(absYaw)>160)
						forwardSpeed=(rotationDist-dist)/2;
					else
					{
						forwardSpeed=0;
						if (absYaw>=0)
							turnSpeed=-turningspeed;
						if(absYaw<0)
							turnSpeed=turningspeed;
						if(fabs(absYaw)>150)
							turnSpeed=turnSpeed*0.25;
					}
					//ora absYaw contiene la rotaz da fare per mettersi in direzione opposta al target

				}
			}

		}
		//this->avoidObstacles(&forwardSpeed, &turnSpeed, *this->sonarProxy, false);
	}
	else
	{
		this->p2dProxy->SetSpeed(0, dtor(0));
		cout<< this->dalekName << " not clear, sleeps "<<endl;
		sleep(10);
		return;
	}

	//set motors
	if(forwardSpeed>maxForwardSpeed)
		forwardSpeed=maxForwardSpeed;

	this->p2dProxy->SetSpeed(forwardSpeed, dtor(turnSpeed));

}

bool Dalek::checkClearPath(double rotDist, double margin)
{
	double otherPlacedist, placedist;
	double otherX, otherY, otherYaw;
	const double safeZoneDist=0.6;//distanza dal formationPlace entro cui questa funzione non chiede lo shift a chi si trova nella nextposition
	//double robodist;
	double nextX,nextY;
	getNextFormationPos(&nextX,&nextY);
	if(this->myCoord.x<this->docCoord.x)
	{
		if(this->myCoord.y<this->docCoord.y) //robo nel quadrante basso sin
		{
			this->dalekQuadrant=lowleft;
		}
		else //quadrante alto sin
		{
			this->dalekQuadrant=hileft;

		}
	}
	else
	{
		if(this->myCoord.y<this->docCoord.y) //quad basso dex
		{this->dalekQuadrant=lowright;
		}
		else //quad alto dex
		{this->dalekQuadrant=hiright;
		}
	}

	//const double securitydistance=1.55;
	placedist=sqrt(((this->formationPlace.x-this->myCoord.x)*(this->formationPlace.x-this->myCoord.x))+((this->formationPlace.y-this->myCoord.y)*(this->formationPlace.y-this->myCoord.y)));
	for(list<otherstates_t>::iterator it = this->dalekNames.begin(); it !=  this->dalekNames.end(); it++)
	{
		if(placedist>safeZoneDist)
		{
			this->simProxy->GetPose2d((char*)(*it).name.c_str(), otherX, otherY, otherYaw);
			otherPlacedist=sqrt(((nextX-otherX)*(nextX-otherX))+((nextY-otherY)*(nextY-otherY)));
			//	robodist=sqrt(((myX-robotX)*(myX-robotX))+((myY-robotY)*(myY-this->myCoord.y)));
			//  if(placedist<1)
			std::cout<<(char*)(*it).name.c_str()<< " dista "<< otherPlacedist<<" da " <<nextX<<","<<nextY << std::endl;
			if (otherPlacedist<0.1)
			{
				cout<<this->dalekName<<" shift asked to "<<(*it).name<<endl;
				askShiftInFormation((*it).name,nextX,nextY);
				this->state=WAITINFORMANSWER;
				this->broadcastStateFunction(this->state);
				return false;
			}
		}
		switch(this->dalekQuadrant)
		{
		case lowleft:
			if((this->myCoord.x<this->docCoord.x+1)&&(this->myCoord.y<this->docCoord.y+1))
				(*it).inExpQuadrant=true;
			else
				(*it).inExpQuadrant=false;
			break;
		case lowright:
			if((this->myCoord.x>this->docCoord.x-1)&&(this->myCoord.y<this->docCoord.y+1))
				(*it).inExpQuadrant=true;
			else
				(*it).inExpQuadrant=false;
			break;
		case hileft:
			if((this->myCoord.x<this->docCoord.x+1)&&(this->myCoord.y>this->docCoord.y-1))
				(*it).inExpQuadrant=true;
			else
				(*it).inExpQuadrant=false;
			break;
		case hiright:
			if((this->myCoord.x>this->docCoord.x-1)&&(this->myCoord.y>this->docCoord.y-1))
				(*it).inExpQuadrant=true;
			else
				(*it).inExpQuadrant=false;
			break;
		default:
			break;
		}
		//	if(robodist<securitydistance)//rischio collisione!
		//	{}
	}
	return true;
}
void Dalek::getNextFormationPos(double* x,double* y)//tested
{
	if(this->myCoord.x<this->docCoord.x)
	{
		if(this->myCoord.y<this->docCoord.y)
		{*x=this->docCoord.x-this->drDistance;
		*y=this->docCoord.y;
		}
		else
		{*x=this->docCoord.x;
		*y=this->docCoord.y+this->drDistance;
		}
	}
	else
	{
		if(this->myCoord.y<this->docCoord.y)
		{*x=this->docCoord.x;
		*y=this->docCoord.y-this->drDistance;
		}
		else
		{*x=this->docCoord.x+this->drDistance;
		*y=this->docCoord.y;
		}
	}
}
void Dalek::askShiftInFormation(string receiver,double x, double y)
{

	//std::cout<<this->dalekName<< " manda shift ("<< x<< ","<< y<<") a: "<< receiver << std::endl;
	message_t mess=shift;
	writeOnPipe(mess,receiver, x, y);
}
void Dalek::askStateFunction(string receiver)
{

	message_t mess=askState;
	writeOnStatePipe(mess,receiver,0,0);
}
void Dalek::broadcastStateFunction(states_t s)
{
	message_t mess=ansState;
	writeOnStatePipe(mess,SEND_TO_ALL,s,0);
}
void Dalek::updateStateFunction(string sender, double statex)
{
	for(list<otherstates_t>::iterator it = this->dalekNames.begin(); it !=  this->dalekNames.end(); it++)
	{
		if(sender == (*it).name )
		{
			switch ((int)statex)
			{
			case 0: (*it).state=WANDERING;break;
			case 1: (*it).state=FORMATION;break;
			case 2: (*it).state=FORMATIONFARAWAY;break;
			case 3: (*it).state=WAITINFORMATION;break;
			case 4: (*it).state=STOP;break;
			case 5: (*it).state=WAITINFORMANSWER;break;
			case 6: (*it).state=IFOUNDDR;break;
			case 7: (*it).state=IFOUNDCAB;break;
			case 8: (*it).state=DOCFOUND;break;
			case 9: (*it).state=CABFOUND;break;
			case 10: (*it).state=CHECKDRPOSITION;break;
			case 11: (*it).state=SENSORTEST;break;
			case 12: (*it).state=SEENDOCTOR;break;
			case 13: (*it).state=SEENCABIN;break;
			case 14: (*it).state=WAITOTHERSAWAY;break;
			case 15: (*it).state=FORMATIONORIENTATION;break;
			case 16: (*it).state=FORMATIONORIENTED;break;
			case 17: (*it).state=FORMATIONGO;break;
			case 18: (*it).state=WAITINGFORFORMATION;break;
			case 19: (*it).state=PaskTest;break;
			default: break;
			}
		}
	}
}

string Dalek::stateToString( double statex)
{
	switch ((int)statex)
	{
	case 0: return string("WANDERING");
	case 1: return string("FORMATION");
	case 2: return string("FORMATIONFARAWAY");
	case 3: return string("WAITINFORMATION");
	case 4: return string("STOP");
	case 5: return string("WAITINFORMANSWER");
	case 6: return string("IFOUNDDR");
	case 7: return string("IFOUNDCAB");
	case 8: return string("DOCFOUND");
	case 9: return string("CABFOUND");
	case 10: return string("CHECKDRPOSITION");
	case 11: return string("SENSORTEST");
	case 12: return string("SEENDOCTOR");
	case 13: return string("SEENCABIN");
	case 14: return string("WAITOTHERSAWAY");
	case 15: return string("FORMATIONORIENTATION");
	case 16: return string("FORMATIONORIENTED");
	case 17: return string("FORMATIONGO");
	case 18: return string("WAITINGFORFORMATION");
	case 19: return string("PaskTest");
	default: return string("dafuq?");
	}
}

int Dalek::easyNavigate(int counter)
{
	bool obstacles=false;

	double dist=sqrt(((this->cabCoord.x-this->docCoord.x)*(this->cabCoord.x-this->docCoord.x))+((this->cabCoord.y-this->docCoord.y)*(this->cabCoord.y-this->docCoord.y)));
	cout<<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> EASY NAVIGATE distance: "<<dist<<endl;
	if (dist<=0.5) //controlla se win
	{
		this->writeOnPipe(goToStop,SEND_TO_ALL,0,1);
		this->state=STOP;
		this->broadcastStateFunction(this->state);
		return 0;
	}

	obstacles=this->naviCheckDir(this->formationYaw);
	if(obstacles)
	{
		cout<<this->dalekName<<" ostacolo trovato a gradi: "<<this->formationYaw<<" CHIAMANDO NAVIGATE"<<endl;
		this->navigate();
		counter=0;
		//this->stopFormation();
	}
	else
	{
		//mantieni l'attuale direzione
		counter++;
	}
	return counter;
}

void Dalek::navigate ()//todo to fix
{
	const int SIDEMARGIN=1;
	const int COOLDOWN=4+5;
	const int PATIENCE=2;
	const int GOALDIST=1;
	const int TURNPROB=6;//se va a rand si gira in un caso su turnprob
	const int BASETURNPROB=10;
	const int RANDPROB=5; //se trova un ostacolo si sposta sulla dir non-pref. con p=1-randprob/BASERAND
	const int BASERAND=10;
	double  dist;
	int nextYaw, randYaw;
	bool obstacle =false;
	bool goOnX=false;

	dist=sqrt(((this->cabCoord.x-this->docCoord.x)*(this->cabCoord.x-this->docCoord.x))+((this->cabCoord.y-this->docCoord.y)*(this->cabCoord.y-this->docCoord.y)));
	cout<<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>distance: "<<dist<<endl;
	if (dist<=GOALDIST) //controlla se win
	{
		this->writeOnPipe(goToStop,SEND_TO_ALL,0,1);
		this->state=STOP;
		this->broadcastStateFunction(this->state);
		return;
	}
	//GESTIONE CONTATORE

	if (dist>=this->bestsofar)  //gestisce il contatore
	{
		this->localminimaCount++;
	}
	else
	{
		this->localminimaCount=0;
		this->bestsofar=dist;
	}
	//VALUTAZIONE COMPORTAMENTO

	if(this->localminimaCount<PATIENCE)  //se è ancora patient, sceglie la rotta ottimale
	{
		//DECIDE NEXTYAW preferenziale
		goOnX=fabs(this->cabCoord.x-this->docCoord.x)<fabs(this->cabCoord.y-this->docCoord.y);
		if(goOnX)
		{
			goOnX=fabs(this->cabCoord.x-this->docCoord.x)>SIDEMARGIN;
		}
		else
		{
			goOnX=fabs(this->cabCoord.y-this->docCoord.y)<SIDEMARGIN;
		}

		if(goOnX)
		{
			if(this->cabCoord.x<this->docCoord.x)
			{
				nextYaw=180;
			}
			else
			{
				nextYaw=0;
			}
		}
		else
		{
			if(this->cabCoord.y<this->docCoord.y)
			{
				nextYaw=270;
			}
			else
			{
				nextYaw=90;
			}
		}
		obstacle = naviCheckDir(nextYaw); //controlla se ci sono ostacoli

		if(obstacle)
		{  //se ci sono cabia rotta
			if(goOnX)
			{
				if(this->cabCoord.y<this->docCoord.y)
				{
					nextYaw=270;
				}
				else
				{
					nextYaw=90;
				}
			}
			else
			{
				if(this->cabCoord.x<this->docCoord.x)
				{
					nextYaw=180;
				}
				else
				{
					nextYaw=0;
				}
			}
		}

		obstacle =naviCheckDir(nextYaw);//controlla se ci sono ostacoli nella nuova dir

		if(!obstacle)
		{
			if (fabs(this->formationYaw-nextYaw)>1)//sarebbe if formationYaw==nextYaw ma i float non supportano ==
			{
				cout<<">>>>>>>> in navigate NEXT_YAW sent: "<<nextYaw<<endl;
				this->writeOnPipe(sendFormationYaw,SEND_TO_ALL,nextYaw,-1);
				this->formationOrientationFlag=correctposition;
				this->oldFormationYaw=this->formationYaw;
				this->formationYaw=nextYaw;
				this->state=FORMATIONORIENTATION;
				this->broadcastStateFunction(this->state);
			}
			return;
		}
		else
		{ //in caso di ostacolo anche sulla seconda direzione con p=1-(randprob/BASERAND)********ok
			if(rand()%BASERAND>RANDPROB)
			{
				obstacle=true;
				while(obstacle) //trova una dir casuale su cui non ci sono ostacoli
				{
					randYaw=this->getRandomDirection();//(rand()%4)*90;
					obstacle =naviCheckDir(randYaw);//controlla se ci sono ostacoli nella nuova dir
				}

				if (fabs(this->formationYaw-randYaw)>1)//sarebbe if formationYaw==nextYaw ma i float non supportano ==
				{
					cout<<">>>>>>>> in navigate RAND_YAW sent: "<<randYaw<<endl;
					this->writeOnPipe(sendFormationYaw,SEND_TO_ALL,randYaw,-1);
					this->oldFormationYaw=this->formationYaw;
					this->formationOrientationFlag=correctposition;
					this->formationYaw=randYaw;
					this->state=FORMATIONORIENTATION;
					this->broadcastStateFunction(this->state);
				}

			}
			else
			{
				obstacle=this->naviCheckDir(this->formationYaw);
				if(obstacle)
				{
					cout<<">>>>>>>> STOP! "<<this->formationYaw<<endl;
					this->stopFormation();
					this->localminimaCount++;//se si deve fermare diventa "impaziente" più in fretta
				}
				else
				{
					cout<<">>>>>>>>>>>>>>>>> tiro dritto"<<endl;
					return;
					//todo tira dritto!
				}
			}
			return;
		}
		return;
	}

	if(this->localminimaCount>COOLDOWN)
	{
		this->bestsofar=dist;
		this->localminimaCount=0;
	}
	else
	{
		//CASO PATIENCE<localminimacount<COOLDOWN
		if(rand()%BASETURNPROB>TURNPROB) //se ci si deve girare******OK
		{
			obstacle=true;
			while(obstacle) //trova una dir casuale su cui non ci sono ostacoli
			{
				randYaw=this->getRandomDirection();//(rand()%4)*90;
				obstacle =naviCheckDir(randYaw);//controlla se ci sono ostacoli nella nuova dir
			}
			if (fabs(this->formationYaw-randYaw)>1) //se la dir è diversa da quella attuale manda un broadcast
			{
				cout<<">>>>>>>> in navigate RANYAW2 sent: "<<randYaw<<endl;
				this->writeOnPipe(sendFormationYaw,SEND_TO_ALL,randYaw,-1);
				this->oldFormationYaw=this->formationYaw;
				this->formationYaw=randYaw;
				this->formationOrientationFlag=correctposition;
				this->state=FORMATIONORIENTATION;
				this->broadcastStateFunction(this->state);
				return;
			}
		}
		else //se non si deve girare
		{

			obstacle =naviCheckDir(this->formationYaw);//controlla se ci sono ostacoli nella dir attuale

			if(obstacle)
			{
				this->stopFormation();
				cout<<">>>>>>>> STOP2! "<<endl;
				this->localminimaCount++;//se si deve fermare diventa "impaziente" più in fretta
			}
			else
			{
				cout<<this->dalekName<<" TIRA DRITTO 2!!!"<<endl;
			}
		}
	}
}

int Dalek::getRandomDirection()
{
	//somma delle coordinate moltiplicate per 100 e farla modulo 4 per 90
	randomize;
	int b=random(100);
	//cout<<"----------------------------------------------------------------\n"<<this->dalekName<<" randomizing: "<<b;
	b=b%4;
//	if(b%2!=0)
//		b--;
	//cout<<" b%4: "<<b;

	b*=90;
	//cout<<" GRADI: "<<b<<"\n------------------------------------------------------------------"<<endl;
	//c=a > b ? a:b;
	return (b);
}

void Dalek::stopFormation()
{
	this->writeOnPipe(sendFormationYaw,SEND_TO_ALL,1000,-1);
	this->state=PaskTest;
	this->broadcastStateFunction(this->state);
	return;
}

//stabilisce sulla base della propria posizione rispetto al dottore quale ruolo assume nella formazione
void Dalek::checkRole()
{
	//normalizzazione coordinate in modo che doc ==(0,0)
	this->getDalekDir();
	switch(this->mydir)
	{
	case top:
		if(fabs(this->formationYaw)<10)
		{
			this->formationPosition=lefth;
		}
		if(fabs(this->formationYaw -90)<10)
		{
			this->formationPosition=anterior;
		}
		if(fabs(this->formationYaw-180)<10)
		{
			this->formationPosition=righth;
		}
		if(fabs(this->formationYaw -270)<10)
		{
			this->formationPosition=posterior;
		}
		break;
	case bot:
		if(fabs(this->formationYaw)<10)
		{
			this->formationPosition=righth;
		}
		if(fabs(this->formationYaw -90)<10)
		{
			this->formationPosition=posterior;
		}
		if(fabs(this->formationYaw-180)<10)
		{
			this->formationPosition=lefth;
		}
		if(fabs(this->formationYaw -270)<10)
		{
			this->formationPosition=anterior;
		}
		break;
	case sx:
		if(fabs(this->formationYaw)<10)
		{
			this->formationPosition=posterior;
		}
		if(fabs(this->formationYaw -90)<10)
		{
			this->formationPosition=lefth;
		}
		if(fabs(this->formationYaw-180)<10)
		{
			this->formationPosition=anterior;
		}
		if(fabs(this->formationYaw -270)<10)
		{
			this->formationPosition=righth;
		}
		break;
	case dx:
		if(fabs(this->formationYaw)<10)
		{
			this->formationPosition=anterior;
		}
		if(fabs(this->formationYaw -90)<10)
		{
			this->formationPosition=righth;
		}
		if(fabs(this->formationYaw-180)<10)
		{
			this->formationPosition=posterior;
		}
		if(fabs(this->formationYaw -270)<10)
		{
			this->formationPosition=lefth;
		}
		break;
	default :
		break;
	}
	cout<<this->dalekName<<"ha ruolo: "<<this->roleToString(this->formationPosition)<<endl;

	/**
	 *
	double docXp=this->docCoord.x+OFFPOSITIONTH;
	double docXm=this->docCoord.x-OFFPOSITIONTH;
	double docYp=this->docCoord.y+OFFPOSITIONTH;
	double docYm=this->docCoord.y-OFFPOSITIONTH;

	switch((int)this->formationYaw)
	{
	case 90:
		if((this->myCoord.x<docXp)&&(this->myCoord.x>docXm)) //caso anteriore OR posteriore
		{
			if(this->myCoord.y>this->docCoord.y) //caso anteriore
			{
				this->formationPosition=anterior;
			}else if (this->myCoord.y<this->docCoord.y){ //caso posteriore
				this->formationPosition=posterior;
			}
		}else
		{ //caso destro OR sinistro
			if(this->myCoord.x>this->docCoord.x) //caso dx
			{
				this->formationPosition=righth;
			}else{ //caso sx
				this->formationPosition=lefth;
			}
		}

		break;
	case 0:
		if((this->myCoord.y>docYm)&&(this->myCoord.y<docYp)) //caso ant e pos
		{
			if(this->myCoord.x>this->docCoord.x)//ant
			{
				this->formationPosition=anterior;
			}else{ //caso posteriore
				this->formationPosition=posterior;
			}
		}else{
			if(this->myCoord.y>this->docCoord.y) //caso sx
			{
				this->formationPosition=lefth;
			}else{ //caso dx
				this->formationPosition=righth;
			}
		}

		break;
	case 270:
		if((this->myCoord.x<docXp)&&(this->myCoord.x>docXm)) //caso anteriore OR posteriore
		{
			if(this->myCoord.y>this->docCoord.y) //caso posteriore
			{
				this->formationPosition=posterior;
			}else{ //caso anteriore
				this->formationPosition=anterior;
			}
		}else{ //caso destro OR sinistro
			if(this->myCoord.x>this->docCoord.x) //caso sx
			{
				this->formationPosition=lefth;
			}else{ //caso dx
				this->formationPosition=righth;
			}
		}

		break;
	case 180:
		if((this->myCoord.y>docYm)&&(this->myCoord.y<docYp)) //caso ant e pos
		{
			if(this->myCoord.x>this->docCoord.x)//pos
			{
				this->formationPosition=posterior;
			}else{ //caso ant
				this->formationPosition=anterior;
			}
		}else{
			if(this->myCoord.y>this->docCoord.y) //caso dx
			{
				this->formationPosition=righth;
			}else{ //caso sx
				this->formationPosition=lefth;
			}
		}
		break;
	default:
		cout << "angle problem in checkRole"<<endl;
		break;
	}

	//cout<<this->dalekName<< " has role "<<this->roleToString(this->formationPosition)<<endl;

	return;

	 */
}

int Dalek::checkObstaclesYaw()
{
	int realYaw=-9999;
	this->simProxy->GetPose2d((char*)this->dalekName.c_str(), this->myCoord.x,this->myCoord.y,this->myCoord.yaw);
	//cout<<">>>>>>>>>>>>>> ";
	if(fabs(this->myCoord.yaw*RADIUSCONSTANTCONVERSION)<10)
	{
		//cout<<this->dalekName << " ritorna 0 con myYaw :"<<this->myCoord.yaw*RADIUSCONSTANTCONVERSION<<endl;
		realYaw=0;
	}
	else if(fabs((this->myCoord.yaw*RADIUSCONSTANTCONVERSION)-90)<10)
	{
		//cout<<this->dalekName << " ritorna 90 con myYaw :"<<this->myCoord.yaw*RADIUSCONSTANTCONVERSION<<endl;
		realYaw=90;
	}
	else if(fabs((this->myCoord.yaw*RADIUSCONSTANTCONVERSION)-180)<10)
	{
		//cout<<this->dalekName << " ritorna 180 con myYaw :"<<this->myCoord.yaw*RADIUSCONSTANTCONVERSION<<endl;
		realYaw=180;
	}
	else if((fabs((this->myCoord.yaw*RADIUSCONSTANTCONVERSION)-270)<10)||(this->myCoord.yaw*RADIUSCONSTANTCONVERSION)<0)
	{
		//cout<<this->dalekName << " ritorna 270 con myYaw :"<<this->myCoord.yaw*RADIUSCONSTANTCONVERSION<<endl;
		realYaw=270;
	}
	return realYaw;
}

string Dalek::roleToString(int role)
{
	switch(role)
	{
	case 0:
		return "ANTERIOR";
	case 1:
		return "POSTERIOR";
	case 3:
		return "SINISTRO";
	case 2:
		return "DESTRO";
	default:
		return "NON IDENTIFICATO";
	}
}

/*
 * ritorna true se è andata bene l'esecuzione
 * ritorna false se c'è stato un errore nella lettura dei sensori
 * */
bool Dalek::checkObstacles()
{
	obstacles_t obt;
	string s="none";
	obt=this->initObstaclesPosition(obt);
	//int robotYaw=this->checkObstaclesYaw();
	int robotYaw=this->getDalek90Yaw();
	switch(this->formationPosition)
	{
	case anterior:
		obt=this->areSensorsAnteriorFree(robotYaw);
		break;
	case lefth:
		obt=this->areSensorsFormationLeftFree(robotYaw);
		break;
	case righth:
		obt=this->areSensorsFormationRightFree(robotYaw);
		break;
	case posterior:
		obt=this->areSensorsFormationPosteriorFree(robotYaw);
		break;
	default:
		cout<<this->dalekName<<" problem on formationposition in checkobstacles"<<endl;
		break;
	}
	if(!obt.sensorError)
	{
		if((this->imNavigator) && (this->formationPosition!=posterior))
		{
			if(this->dalekName=="dalekCan")
			{
				obt.can=1;
			}
			if(this->dalekName=="dalekSec")
			{
				obt.sec=1;
			}
			if(this->dalekName=="dalekThay")
			{
				obt.thay=1;
			}
			if(this->dalekName=="dalekJast")
			{
				obt.jast=1;
			}
		}
		this->obstaclesPosition=obt;
		if(checkObstaclesFlag(obt))
		{
			//this->obstaclesPosition=obt;
			double x=0;
			if(obt._0)
			{
				s="0";
				//cout<<this->dalekName<<" TROVATO ostacolo a 0"<<endl;
				x+=1000;
			}
			if(obt._90)
			{
				s+="+90";
				x+=100;
				//cout<<this->dalekName<<" TROVATO ostacolo a 90"<<endl;

			}
			if(obt._180)
			{
				s+="+180";
				x+=10;
				//cout<<this->dalekName<<" TROVATO ostacolo a 180"<<endl;

			}
			if(obt._270)
			{
				s+="+270";
				x+=1;
				//cout<<this->dalekName<<" TROVATO ostacolo a 270"<<endl;

			}
//			cout<<this->dalekName<<" in posizione: "<< this->roleToString(this->formationPosition)<<" manda obstacles -> "<<x<<" = "<<s<<endl;
			this->writeOnPipe(obstacles,SEND_TO_ALL,x,0.0);
			return true;
		}
		else
		{
			//cout<<this->dalekName<<" manda obstacles ";
			this->writeOnPipe(obstacles,SEND_TO_ALL,0.0,0.0);
			return true;
		}
	}
	else
	{
		cout<<this->dalekName<<" in Position: "<<this->roleToString(this->formationPosition)<<" ERRORE: lettura sensori!"<<endl;
		return false;
	}
	return true;
}

obstacles_t Dalek::areSensorsAnteriorFree(int cfrYaw)
{
	obstacles_t obstaclesPosition;
	obstaclesPosition=this->initObstaclesPosition(obstaclesPosition);
	const double OBSTACLEFRONTSXDISTANCE=1; //una soglia differente per ogni sonar
	const double OBSTACLEFRONTDXDISTANCE=1;
	const double OBSTACLELATERALSXDISTANCE=2.1;
	const double OBSTACLELATERALDXDISTANCE=2.1;

	//prendiamo solo i sx perchè stiamo effettuando un movimento rotatorio orario
	double toothsx=this->sonarProxy->GetScan(0);     //dente sx
	double toothdx=this->sonarProxy->GetScan(1);     //dente dx
	double lateralsx=this->sonarProxy->GetScan(7);   //laterale sx
	double lateraldx=this->sonarProxy->GetScan(8);  //laterale dx
	if((toothsx<0)||(toothdx<0)||(lateralsx<0)||(lateraldx<0))
	{
		obstaclesPosition.sensorError=true;
		cout<<this->dalekName<<" ERRORE: sensori anteriori minori di 0!"<<endl;
		return obstaclesPosition;
	}
	if((toothsx<OBSTACLEFRONTSXDISTANCE)||(toothdx<OBSTACLEFRONTDXDISTANCE))
	{
		//cout<<this->dalekName<<" ruolo: "<<this->formationPosition<<" toothDx: "<<toothdx<<" toothSx: "<<toothsx<<endl;
		switch (cfrYaw)
		{
		case 180:
			obstaclesPosition._180=true;
			break;
		case 0:
			obstaclesPosition._0=true;
			break;
		case 270:
			obstaclesPosition._270=true;
			break;
		case 90:
			obstaclesPosition._90=true;
			break;
		}
		//this->goStraight(0.0);
	}
	if(lateralsx<OBSTACLELATERALSXDISTANCE)
	{
		//cout<<this->dalekName<<" ruolo: "<<this->formationPosition<<" OSTACOLO lateralSx: "<<lateralsx<<endl;
		switch (cfrYaw+90)
		{
		case 180:
			obstaclesPosition._180=true;
			break;
		case 360:
		case 0:
			obstaclesPosition._0=true;
			break;
		case 270:
			obstaclesPosition._270=true;
			break;
		case 90:
			obstaclesPosition._90=true;
			break;
		}
	}
	if(lateraldx<OBSTACLELATERALDXDISTANCE)
	{
		//cout<<this->dalekName<<" ruolo: "<<this->formationPosition<<" OSTACOLO lateralDx: "<<lateraldx<<endl;
		switch (cfrYaw-90)
		{
		case 180:
			obstaclesPosition._180=true;
			break;
		case 0:
			obstaclesPosition._0=true;
			break;
		case -90:
		case 270:
			obstaclesPosition._270=true;
			break;
		case 90:
			obstaclesPosition._90=true;
			break;
		}
	}
	obstaclesPosition.sensorError=false;
	return obstaclesPosition;
}

obstacles_t Dalek::areSensorsFormationPosteriorFree(int cfrYaw)
{
	obstacles_t obstaclesPosition;
	obstaclesPosition=this->initObstaclesPosition(obstaclesPosition);
	const double OBSTACLEASSSXDISTANCE=2.75; //una soglia differente per ogni sonar
	const double OBSTACLEASSDXDISTANCE=2.75;

	//prendiamo solo i sx perchè stiamo effettuando un movimento rotatorio orario
	double culosx=this->sonarProxy->GetScan(10);     //culo sx
	double culodx=this->sonarProxy->GetScan(11);     //culo dx

	if((culosx<0)||(culodx<0))
	{
		obstaclesPosition.sensorError=true;
		cout<<this->dalekName<<" ERRORE: SENSORI ANALI VALORE MINORE DI 0"<<endl;
		return obstaclesPosition;
	}
	//cout<<this->dalekName<<" ruolo: "<<this->formationPosition<<" culosx: "<<culosx<<" culodx: "<<culodx<<endl;
	if((culosx<OBSTACLEASSSXDISTANCE)||(culodx<OBSTACLEASSDXDISTANCE))
	{
		//cout<<this->dalekName<<" ruolo: "<<this->formationPosition<<" OSTACOLO NELL'ANO SX: "<<culosx<<endl;
		//cout<<this->dalekName<<" ruolo: "<<this->formationPosition<<" OSTACOLO NELL'ANO DX: "<<culodx<<endl;
		//cout<<this->dalekName<<" form Yaw: "<<this->formationYaw<<" +180: "<<this->formationYaw+180<<endl;
		switch (cfrYaw+180)
		{
		case 180:
			//cout<<"ostacolo a 180!"<<endl;
			obstaclesPosition._180=true;
			break;
		case 360:
		case 0:
			//cout<<"ostacolo a 360 || 0!"<<endl;
			obstaclesPosition._0=true;
			break;
		case 270:
			//cout<<"ostacolo a 270!"<<endl;
			obstaclesPosition._270=true;
			break;
		case 450:
		case 90:
			//cout<<"ostacolo a 90!"<<endl;
			obstaclesPosition._90=true;
			break;
		}
	}
	obstaclesPosition.sensorError=false;
	return obstaclesPosition;
}

obstacles_t Dalek::areSensorsFormationLeftFree(int cfrYaw)
{
	obstacles_t obstaclesPosition;
	obstaclesPosition=this->initObstaclesPosition(obstaclesPosition);
	const double OBSTACLEFRONTSXDISTANCE=2.75; //una soglia differente per ogni sonar
	const double OBSTACLEFRONTDXDISTANCE=2.75;
	const double OBSTACLELATERALSXDISTANCE=2.75;

	//prendiamo solo i sx perchè stiamo effettuando un movimento rotatorio orario
	double toothsx=this->sonarProxy->GetScan(0);     //dente sx
	double toothdx=this->sonarProxy->GetScan(1);     //dente dx
	double lateralsx=this->sonarProxy->GetScan(7);   //laterale sx

	if((toothsx<0)||(toothdx<0)||(lateralsx<0))
	{
		cout<<this->dalekName<<" ERRORE: sensori sinistri minori di 0!"<<endl;
		obstaclesPosition.sensorError=true;
		return obstaclesPosition;
	}

	if((toothsx<OBSTACLEFRONTSXDISTANCE)||(toothdx<OBSTACLEFRONTDXDISTANCE))
	{
		//cout<<this->dalekName<<" ruolo: "<<this->formationPosition<<" OSTACOLO toothDx: "<<toothdx<<" OSTACOLO toothSx: "<<toothsx<<endl;
		switch (cfrYaw)
		{
		case 180:
			obstaclesPosition._180=true;
			break;
		case 0:
			obstaclesPosition._0=true;
			break;
		case 270:
			obstaclesPosition._270=true;
			break;
		case 90:
			obstaclesPosition._90=true;
			break;
		}
		//this->goStraight(0.0);
	}
	if(lateralsx<OBSTACLELATERALSXDISTANCE)
	{
		//cout<<this->dalekName<<" ruolo: "<<this->formationPosition<<" OSTACOLO lateralSx: "<<lateralsx<<endl;
		switch (cfrYaw+90)
		{
		case 180:
			obstaclesPosition._180=true;
			break;
		case 360:
		case 0:
			obstaclesPosition._0=true;
			break;
		case 270:
			obstaclesPosition._270=true;
			break;
		case 90:
			obstaclesPosition._90=true;
			break;
		}
	}
	obstaclesPosition.sensorError=false;
	return obstaclesPosition;
}

obstacles_t Dalek::areSensorsFormationRightFree(int cfrYaw)
{
	obstacles_t obstaclesPosition;
	obstaclesPosition=this->initObstaclesPosition(obstaclesPosition);
	const double OBSTACLEFRONTSXDISTANCE=2.75; //una soglia differente per ogni sonar
	const double OBSTACLEFRONTDXDISTANCE=2.75;
	const double OBSTACLELATERALDXDISTANCE=2.75;

	//prendiamo solo i sx perchè stiamo effettuando un movimento rotatorio orario
	double toothsx=this->sonarProxy->GetScan(0);     //dente sx
	double toothdx=this->sonarProxy->GetScan(1);     //dente dx
	double lateraldx=this->sonarProxy->GetScan(8);  //laterale dx

	if((toothsx<0)||(toothdx<0)||(lateraldx<0))
	{
		cout<<this->dalekName<<" ERRORE: sensori sinistri minori di 0!"<<endl;
		obstaclesPosition.sensorError=true;
		return obstaclesPosition;
	}

	if((toothsx<OBSTACLEFRONTSXDISTANCE)||(toothdx<OBSTACLEFRONTDXDISTANCE))
	{
		//cout<<this->dalekName<<" ruolo: "<<this->formationPosition<<" OSTACOLO toothDx: "<<toothdx<<" OSTACOLO toothSx: "<<toothsx<<endl;
		switch (cfrYaw)
		{
		case 180:
			obstaclesPosition._180=true;
			break;
		case 0:
			obstaclesPosition._0=true;
			break;
		case 270:
			obstaclesPosition._270=true;
			break;
		case 90:
			obstaclesPosition._90=true;
			break;
		}
		//this->goStraight(0.0);
	}
	if(lateraldx<OBSTACLELATERALDXDISTANCE)
	{
		//cout<<this->dalekName<<" ruolo: "<<this->roleToString(this->formationPosition)<<" OSTACOLO lateralDx: "<<lateraldx<<" CFRYAW: " << cfrYaw<< " -90: "<<cfrYaw-90 <<endl;
		switch (cfrYaw-90)
		{
		case 180:
			obstaclesPosition._180=true;
			break;
		case 0:
			obstaclesPosition._0=true;
			break;
		case -90:
		case 270:
			obstaclesPosition._270=true;
			break;
		case 90:
			obstaclesPosition._90=true;
			break;
		}
	}
	obstaclesPosition.sensorError=false;
	return obstaclesPosition;
}

bool Dalek::checkObstaclesFlag(obstacles_t obt)
{
	if((obt._0)||(obt._180)||(obt._270)||(obt._90))
		return true;
	return false;
}

obstacles_t Dalek::initObstaclesPosition(obstacles_t obt)
{
	obt.can=0;
	obt.sec=0;
	obt.jast=0;
	obt.thay=0;
	obt._0=false;
	obt._90=false;
	obt._180=false;
	obt._270=false;
	obt.sensorError=false;
	//cout<<" >>>> obstacles resetted! STABBE'! <<<<"<<endl;
	return obt;
}

int Dalek::formationKeeper()
{
	//int result;
	//const double BOTDIFF=this->drDistance,TOPDIFF=this->drDistance;
	bool coordy;
	double  robotX, robotY, robotYaw, tempcoord;
	double safetydist;
	cordinates_s top, bot, right,left;
	double docXp=this->docCoord.x+OFFPOSITIONTH;
	double docXm=this->docCoord.x-OFFPOSITIONTH;
	double docYp=this->docCoord.y+OFFPOSITIONTH;
	double docYm=this->docCoord.y-OFFPOSITIONTH;

	switch(this->formationPosition)
	{
	case anterior:
		safetydist=2.5;
		break;
	case lefth:
		safetydist=2.5;
		break;
	case righth:
		safetydist=2.5;
		break;
	case posterior:
		//safetydist=1.7;
		safetydist=0.7;
		break;
	default:
		cout<<this->dalekName<<" problem on formationposition in formkeeper"<<endl;
		break;
	}

	for(list<otherstates_t>::iterator it = this->dalekNames.begin(); it !=  this->dalekNames.end(); it++)
	{
		this->simProxy->GetPose2d((char*)(*it).name.c_str(), robotX, robotY, robotYaw);

		switch((int)this->formationYaw)
		{
		case 90:
			if((robotX<docXp)&&(robotX>docXm)) //caso anteriore OR posteriore
			{
				if(robotY>this->docCoord.y) //caso anteriore
				{
					top.x=robotX;
					top.y=robotY;
				}else{ //caso posteriore
					bot.x=robotX;
					bot.y=robotY;
				}
			}else

			{ //caso destro OR sinistro
				if(robotX>this->docCoord.x) //caso dx
				{
					right.x=robotX;
					right.y=robotY;
				}else{ //caso sx
					left.x=robotX;
					left.y=robotY;
				}
			}
			break;
		case 0:
			if((robotY>docYm)&&(robotY<docYp)) //caso ant e pos
			{
				if(robotX>this->docCoord.x)//ant
				{
					top.x=robotX;
					top.y=robotY;
				}else{ //caso posteriore
					bot.x=robotX;
					bot.y=robotY;
				}
			}else
			{
				if(robotY>this->docCoord.y) //caso sx
				{
					left.x=robotX;left.y=robotY;
				}else{ //caso dx
					right.x=robotX;right.y=robotY;
				}
			}
			break;
		case 270:
			if((robotX<docXp)&&(robotX>docXm)) //caso anteriore OR posteriore
			{
				if(robotY>this->docCoord.y) //caso posteriore
				{
					bot.x=robotX;
					bot.y=robotY;
				}else{ //caso anteriore
					top.x=robotX;
					top.y=robotY;
				}
			}else
			{ //caso destro OR sinistro
				if(robotX>this->docCoord.x) //caso sx
				{
					left.x=robotX;
					left.y=robotY;
				}else{ //caso dx
					right.x=robotX;
					right.y=robotY;
				}
			}
			break;
		case 180:
			if((robotY>docYm)&&(robotY<docYp)) //caso ant e pos
			{
				if(robotX>this->docCoord.x)//pos
				{
					bot.x=robotX;bot.y=robotY;
				}else{ //caso ant
					top.x=robotX;top.y=robotY;
				}
			}else
			{
				if(robotY>this->docCoord.y) //caso dx
				{
					right.x=robotX;right.y=robotY;
				}else{ //caso sx
					left.x=robotX;left.y=robotY;
				}
			}
			break;
		default:
			cout << "angle problem in checkRole"<<endl;
			break;
		}
	}
	switch((int)this->formationYaw)
	{
	case 90:
	case 270:
		coordy=true;
		break;
	case 0:
	case 180:
		coordy=false;
		break;
	default:
		cout<<this->dalekName<<" ERROR IN FORMATION KEEPER"<<endl;
		break;
	}

	robotX = this->myCoord.x, robotY=this->myCoord.y, robotYaw=this->myCoord.yaw;
	if(coordy)
		tempcoord=robotY;
	else
		tempcoord=robotX;
	//cout<<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>"<<endl;
	//cout<<this->dalekName<<" tempcoord: "<<tempcoord<<" formationposition: "<<this->roleToString(this->formationPosition)<<endl;
	switch(isLateOrTooForward(tempcoord, this->formationPosition))
	{//bisogna aggiungere al too far la possibilità di dire "torna indietro" avanza=1, torna indietro=-1 fermo =0
	case-1: //is late, keep going
		if((this->sonarProxy->GetScan(0)>=safetydist)&&(this->sonarProxy->GetScan(1)>=safetydist))
		{
			cout<<this->dalekName<<" is late"<<endl;
			return 1;
		}
		else {
			cout<<this->dalekName<<">>>>>>>> sensor ##first## STOP! "<<this->formationYaw<<endl;
			this->stopFormation();
			return 0;}
		break;
	case 2://extra far, go back

		//cout<<this->dalekName<<" ("<<this->myCoord.x<<","<<this->myCoord.y<<")"<<" is extra far  doc is: ("<<this->docCoord.x<<","<<this->docCoord.y<<")"<<endl;
		return -1;
		break;
	case 1: //too far, wait

		//cout<<this->dalekName<<" is too far"<<endl;
		return 0;

	case 0: //check if to wait someone
		if(this->formationPosition!=anterior ){
			if(coordy)
			{tempcoord=top.y;}
			else
			{tempcoord=top.x;}
			if(isLateOrTooForward(tempcoord, anterior)==-1)
			{
				//cout<<this->dalekName<<" wait anterior"<<endl;
				return 0;
			}
		}
		if(this->formationPosition!= posterior){
			if(coordy)
			{tempcoord=bot.y;}
			else
			{tempcoord=bot.x;}
			if(isLateOrTooForward(tempcoord,posterior )==-1)
			{
				cout<<this->dalekName<<" wait posterior"<<endl;
				return 0;
			}
		}
		if(this->formationPosition!= lefth){
			if(coordy)
			{tempcoord=left.y;}
			else
			{tempcoord=left.x;}
			if(isLateOrTooForward(tempcoord, lefth)==-1)
			{
				cout<<this->dalekName<<" wait left"<<endl;
				return 0;
			}
		}
		if(this->formationPosition!= righth){
			if(coordy)
			{tempcoord=right.y;}
			else
			{tempcoord=right.x;}
			if(isLateOrTooForward(tempcoord, righth)==-1)
			{
				cout<<this->dalekName<<" wait right"<<endl;
				return 0;
			}
		}
		break;
	}


	if((this->sonarProxy->GetScan(0)>=safetydist)&&(this->sonarProxy->GetScan(1)>=safetydist))
	{
		cout<<this->dalekName<<" going straight!"<<endl;
		return 1;
	}
	else
	{
		cout<<this->dalekName<< ">>>>>>>> sensor ###last# STOP! "<<this->formationYaw<<endl;
		this->stopFormation();
		return 0;
	}
}

int Dalek::isLateOrTooForward(double tempcoord, position_t role)
{
	const double ADMITTEDDEVIATION=0.5;
	const double late=-1;
	const double fine=0;
	const double tooForw=1;
	const double extraForw=2;
	double distance=0;
	switch ((int)this->formationYaw)
	{
	case 0:
		distance=this->docCoord.x-tempcoord;
		break;
	case 180:
		distance=-(this->docCoord.x-tempcoord);
		break;
	case 90:
		distance=this->docCoord.y-tempcoord;
		break;
	case 270:
		distance=-(this->docCoord.y-tempcoord);
		break;
	}
//	cout<<this->dalekName<< " distance: "<<distance<<endl;
	switch(role)
	{
	case anterior:
		distance+=this->drDistance;
		break;
	case lefth:
		break;
	case righth:
		break;
	case posterior:
		distance-=this->drDistance;
		break;
	}
//	cout<<this->dalekName<< " NEW distance: "<<distance<<endl;
	if(distance<-ADMITTEDDEVIATION)
	{
		if(distance<-(2*ADMITTEDDEVIATION))
		{
			//cout<<this->dalekName<<" islateecc MOLTISSIMO LONTANO <-1"<<endl;
			return extraForw;
		}
		else
		{
			//cout<<this->dalekName<<" islateecc MOLTO LONTANO >=-1"<<endl;
			return tooForw;
		}
	}
	if(distance > ADMITTEDDEVIATION)
	{
		//cout<<this->dalekName<<" islateecc RITARDO >1"<<endl;
		return late;
	}
	//cout<<this->dalekName<<" islateecc APPOST <=-1"<<endl;
	return fine;
}


bool Dalek::stopInCorrectPosition()//necessita mycoord e doccoord
{
	this->pushDr=false;
	const double LONGMARGIN=0.8;
	const double CLOSEMARGIN=0.1;
	double coordinata=-1000;
	double deviation;
	double mycoordinata;
	double VELOCITY=0.1;
	bool result=false;

	//this->simProxy->GetPose2d((char*)"who", this->docCoord.x,this->docCoord.y,this->docCoord.yaw);
	//this->simProxy->GetPose2d((char*)"tardis", this->cabCoord.x,this->cabCoord.y,this->cabCoord.yaw);

	cout<< this->dalekName<<" YAW : "<<getDalek90Yaw()<<endl;
	switch(getDalek90Yaw())
	{
	case 0:
		mycoordinata=this->myCoord.x;
		if(this->formationPosition==anterior)
		{coordinata=this->getFormationCoord(dx,1);}
		if(this->formationPosition==posterior)
		{coordinata=this->getFormationCoord(sx,1);}
		if(this->formationPosition==lefth)
		{coordinata=this->getFormationCoord(top,1);}
		if(this->formationPosition==righth)
		{coordinata=this->getFormationCoord(bot,1);}
		break;

	case 90:
		mycoordinata=this->myCoord.y;
		if(this->formationPosition==anterior)
		{coordinata=this->getFormationCoord(top,0);}
		if(this->formationPosition==posterior)
		{coordinata=this->getFormationCoord(bot,0);}
		if(this->formationPosition==lefth)
		{coordinata=this->getFormationCoord(sx,0);}
		if(this->formationPosition==righth)
		{coordinata=this->getFormationCoord(dx,0);}

		break;
	case 180:
		mycoordinata=this->myCoord.x;
		VELOCITY=VELOCITY*(-1);//todo in un test è risultato che erano invertite le vel... vediamo se va
		if(this->formationPosition==anterior)
		{coordinata=this->getFormationCoord(sx,1);}
		if(this->formationPosition==posterior)
		{coordinata=this->getFormationCoord(dx,1);}
		if(this->formationPosition==lefth)
		{coordinata=this->getFormationCoord(bot,1);}
		if(this->formationPosition==righth)
		{coordinata=this->getFormationCoord(top,1);}
		break;
	case 270:
		mycoordinata=this->myCoord.y;
		VELOCITY=VELOCITY*(-1);
		if(this->formationPosition==anterior)
		{coordinata=this->getFormationCoord(bot,0);}
		if(this->formationPosition==posterior)
		{coordinata=this->getFormationCoord(top,0);}
		if(this->formationPosition==lefth)
		{coordinata=this->getFormationCoord(dx,0);}
		if(this->formationPosition==righth)
		{coordinata=this->getFormationCoord(sx,0);}
		break;
	default:
		cout<<"problem in switch(formationyaw) in stopInCorrectPosition"<<endl;
		break;
	}
	deviation=mycoordinata-coordinata;
	cout<<this->dalekName<<" deviation=mycoordinata-coordinata;"<<endl;
	cout<<this->dalekName<<" "<<deviation<<"="<<mycoordinata<<"-"<<coordinata<<";"<<endl;
	if(deviation<-LONGMARGIN){//avanti
		this->forwardspeed=VELOCITY;result= false;}
	else if(deviation>LONGMARGIN){
		this->forwardspeed=-VELOCITY;result= false;}
	else
	{
		if(deviation<-CLOSEMARGIN){//avanti piano
			this->forwardspeed=VELOCITY*(0.25);result= false;}
		else if(deviation>CLOSEMARGIN){
			this->forwardspeed=-VELOCITY*(0.25);result= false;}
		else
		{
			this->forwardspeed=0.0;
			result= true;
		}
		if(!result)
		{
			this->turnspeed=0.0;
		}
	}

	if(this->forwardspeed<0)
	{
		if(this->formationPosition==anterior)
		{
			switch(getDalek90Yaw())
			{
			case 0:
			case 180:
				deviation=fabs(this->docCoord.x-mycoordinata);
				break;
			case 90:
			case 270:
				deviation=fabs(this->docCoord.y-mycoordinata);
				break;
			}
			if(deviation<1)
				this->forwardspeed=0;
		}
	}
	return result;
}

bool Dalek::naviCheckDir(double nextYaw)
{
	/*cout<<this->dalekName<<" IN NAVICHECKDIR"<<endl<<"0: "<<this->obstaclesPosition._0<<" 90: "<<this->obstaclesPosition._90<<
			" 180: "<<this->obstaclesPosition._180<<" 270: "<<this->obstaclesPosition._270 <<endl;
	 */
	switch ((int)nextYaw)  		//controlla gli ostacoli
	{
	case 0:
		if(this->obstaclesPosition._0)
			return true;
		break;
	case 90:
		if(this->obstaclesPosition._90)
			return true;
		break;
	case 180:
		if(this->obstaclesPosition._180)
			return true;
		break;
	case 270:
		if(this->obstaclesPosition._270)
			return true;
		break;
	default:
		cout<<"in navicheckdir yaw non ammesso"<<endl;
		break;
	}
	return false;
}

double Dalek::yawToTurnTo(double ax,double ay, double bx,double by)
{
	double absYaw;
	if(fabs(ax-bx)>0.01)
		absYaw=atan((ay-by)/(ax-bx))* (57.2957796);
	else
	{
		if(ax-bx>=0)
			absYaw=atan((ay-by)/0.01)* (57.2957796);
		else
			absYaw=atan((ay-by)/-0.01)* (57.2957796);
	}
	if((ax-bx)<0)
	{
		if((ay-by)>0.001)
			absYaw+=180;
		else
			absYaw-=180;
	}


	if(absYaw>0)
		absYaw-=180;
	else
		absYaw+=180;
	return absYaw;
}

int Dalek::angleDiff(double ax,double ay,double ayaw, double bx,double by)//*******TESTATA, ayaw può essere uno yaw fresco di getpose
{ //ottiene la diff tra ayaw e l'angolo ottenuto misurando l'inclinazione del segmento ax,ay - bx,by
	//es se metti come coordinate a quelle di un dalek e b quelle del posto dove deve andare ti restituisce lo yaw di cui girare in gradi

	double absYaw = yawToTurnTo( ax, ay,  bx, by)- (ayaw*RADIUSCONSTANTCONVERSION);


	while(absYaw>=181)
	{absYaw-=360;}
	while(absYaw<=-181)
	{absYaw+=360;}
	return (int)absYaw;
	//TESTATO->absYaw adesso contiene i gradi di cui si deve girare per arrivare a 0° rispetto alla dir. di target
}

double Dalek::getFormationCoord(direction_t dir, bool coordIsX)
{

	double x;

	if (coordIsX){
		switch(dir)
		{
		case top:
		case bot:
			x=this->docCoord.x;
			break;
		case dx:
			x=this->docCoord.x+this->drDistance;
			break;
		case sx:
			x=this->docCoord.x-this->drDistance;
			break;
		}
	}else
	{
		switch(dir)
		{
		case top:
			x=this->docCoord.y+this->drDistance;
			break;
		case bot:
			x=this->docCoord.y-this->drDistance;
			break;
		case dx:
		case sx:
			x=this->docCoord.y;
			break;
		}

	}
	cout<<this->dalekName<<" coordinata restituita per direzione "<<dir<<" e coordisx "<<coordIsX<<" è >>--> "<<x<<endl;
	cout<<this->dalekName<<" dati: "<<this->docCoord.x<<" , "<<this->docCoord.y<<endl;

	return x;

}

void Dalek::getDalekDir()//***************************testato ok
{
	//	double docXp=this->docCoord.x+OFFPOSITIONTH;
	//	double docXm=this->docCoord.x-OFFPOSITIONTH;
	//	cout<<this->dalekName<<" getDakdsiadub "<<endl;
	double mx=this->myCoord.x-this->docCoord.x;
	double my=this->myCoord.y-this->docCoord.y;
	cout<<this->dalekName<<"cercando un ruolo"<<endl;
	cout<<"coordinate: "<<mx<<","<<my<<endl;

	if(my<mx)//sotto la diagonale Y=X -> dx or bot
	{
		if(my<-mx)//sotto la diagonale Y=-X sx or bot
		{
			this->mydir=bot;
		}
		else
		{
			this->mydir=dx;
		}
	}
	else//sopra Y=X->sx or top
	{
		if(my<-mx)//sotto la diagonale Y=-X sx or bot
		{
			this->mydir=sx;
		}
		else
		{
			this->mydir=top;
		}
	}
	/*
	if((this->myCoord.x<docXp)&&(this->myCoord.x>docXm)) //caso anteriore OR posteriore
	{
		if(this->myCoord.y>this->docCoord.y) //caso anteriore
		{
			this->mydir= top;
		}else if (this->myCoord.y<this->docCoord.y){ //caso posteriore
			this->mydir= bot;
		}
	}else
	{ //caso destro OR sinistro
		if(this->myCoord.x>this->docCoord.x) //caso dx
		{
			this->mydir= dx;
		}else{ //caso sx
			this->mydir= sx;
		}
	}
	 */
}


int Dalek::getDalek90Yaw()
{
	cout<<this->dalekName<<">>>>>>>>>>>>>> YAW RAW: "<<this->myCoord.yaw*RADIUSCONSTANTCONVERSION<<endl;
	if(fabs(this->myCoord.yaw*RADIUSCONSTANTCONVERSION)<10)
	{
		//cout<<this->dalekName << " ritorna 0 con myYaw :"<<this->myCoord.yaw*RADIUSCONSTANTCONVERSION<<endl;
		return 0;
	}
	else if(fabs((this->myCoord.yaw*RADIUSCONSTANTCONVERSION)-90)<10)
	{
		//cout<<this->dalekName << " ritorna 90 con myYaw :"<<this->myCoord.yaw*RADIUSCONSTANTCONVERSION<<endl;
		return 90;
	}
	else if(fabs(fabs((this->myCoord.yaw*RADIUSCONSTANTCONVERSION))-180)<10)
	{
		//cout<<this->dalekName << " ritorna 180 con myYaw :"<<this->myCoord.yaw*RADIUSCONSTANTCONVERSION<<endl;
		return 180;
	}
	else if((fabs((this->myCoord.yaw*RADIUSCONSTANTCONVERSION)-270)<10)||(fabs((this->myCoord.yaw*RADIUSCONSTANTCONVERSION)+90)<10))
	{
		//cout<<this->dalekName << " ritorna 270 con myYaw :"<<this->myCoord.yaw*RADIUSCONSTANTCONVERSION<<endl;
		return 270;
	}
	else
	{
		cout<<this->dalekName<<"error in estimating 90 yaw "<<this->myCoord.yaw*RADIUSCONSTANTCONVERSION<<endl;
		return 1000;
	}
}

direction_t Dalek::getNextDir(){//***************************testato ok
	switch(this->mydir){
	case dx:
		return bot;
	case bot:
		return sx;
	case sx:
		return top;
	case top:
		return dx;
	default:
		cout<<this->dalekName<<"direzione non inizializzata in getNextDir"<<endl;
		return top;
	}
}
